Control device for mobile body

ABSTRACT

Ground contact portions are categorized in a tree structure manner such that all of the ground contact portions of a mobile body (mobile robot) equipped with three or more ground contact portions become leaf nodes and that an intermediate node exists between the leaf nodes and a root node having all the leaf nodes as its descendant nodes. On each node (a C-th node) having child nodes, the correction amounts of the desired relative heights of the ground contact portions of the C-th node are determined such that at least the difference between an actual posture inclination and a desired posture inclination of a predetermined portion, such as a base body, (posture inclination difference) is approximated to zero, and joints of the mobile body  1  are operated so that a desired relative height obtained by combining the correction amounts is satisfied.

TECHNICAL FIELD

The present invention relates to the control of a floor reaction forceof a mobile body, such as a legged mobile robot and a wheeled mobilebody having an active suspension. More specifically, the presentinvention relates to a device for properly controlling a floor reactionforce to minimize influences of an unknown slope, projection ordepression of a floor (ground surface) with which a mobile body, such asthe legged mobile robot or the wheeled mobile robot having an activesuspension, comes in contact. Moreover, the present invention relates toa device for properly controlling a floor reaction force whileestimating the configuration of a floor.

BACKGROUND ART

Japanese Unexamined Patent Application Publication No. H5-305585 by thepresent applicant (Patent Document 1) discloses a technology of thecompliance control of both legs of a two-legged mobile robot. Thistechnology is applicable also to a robot having, for example, four legs.In this case, according to the compliance control based on thetechnology disclosed in the publication, a virtual plane is inclinedwhile all legs are in contact with the virtual plane, so that anundulation or tilt over a large area of the floor can be dealt with.According to the technology of the publication, however, there have beencases where a load (floor reaction force) is not properly distributed ifa floor has a local tilt, projection or depression. As a result, poorcontact with a surface sometimes occurred, causing a portion (foot) tobe in contact therewith to shake or to be subjected to an undue landingimpact, with consequent deteriorated properties of foot contact with theground. This further led to a danger in that a robot easily slips orspins. Further, there have been cases where control fails to properlydistribute a floor reaction force moment for restoring the posture ofthe robot to a desired posture to the distal portion of each leg,causing the posture of the robot to shake. There have been also caseswhere, if a certain portion of the robot starts vibrating, then it isinconveniently difficult to attenuate the vibration. Further, there havebeen cases where the failure of proper distribution control of a floorreaction force to the distal end of each leg results in excessive loadon some actuators.

In addition, Japanese Unexamined Patent Application Publication No.H10-277969 (Patent Document 2) and Japanese Unexamined PatentApplication Publication No. 2001-322076 (Patent Document 3) by thepresent applicant disclose a technology for composite-compliance controland a technology for estimating the configuration of a floor. Thesetechnologies make it possible to properly distribute a floor reactionforce to the distal portion of each leg in a two-legged mobile robot andto compositively estimate the configuration of a floor, morespecifically, compositively and accurately estimate the inclination ofthe ground contact surface of each leg distal portion and the heightdifference between ground contact surfaces at the same time. Thesetechnologies in the publications are, in principle, applicable to amulti-legged robot, such as a four-legged robot; however, no descriptionhas been given of a technology for specifically applying them.

Further, there has been known a technique for estimating the height of afloor at the time of landing by using feelers (load sensors or contactsensors) provided at the distal ends of the legs of the four-leggedrobot. However, this technique is a sequential technique and has beenincapable of dealing with a case where a floor deforms after thelanding, as with a soft ground. Especially when the main body of therobot is severely vibrating, such as when the robot is moving fast, theheight of a floor cannot be correctly estimated, and there has been adanger in that load is not properly distributed to each leg.

There has been also known a technique whereby the compliance control ofeach leg of a robot is independently conducted for each leg. Thistechnique, however, does not take into account the fact that the legsinterfere with each other through the vibration of the body of therobot. In other words, the compliance control is individually carriedout on each leg. For this reason, the compliance control of each legdoes not act in harmony, making it impossible to properly distribute afloor reaction force to each leg.

The present invention has been made with a view of the backgrounddescribed above, and it is an object thereof to provide a device forproperly controlling a floor reaction force to minimize influences of anunknown slope, projection or depression of a floor with which a mobilebody, such as a legged mobile robot or a wheeled mobile body having anactive suspension, comes in contact. Moreover, an object of the presentinvention is to provide a device for properly controlling a floorreaction force while accurately estimating the configuration of a floor.

DISCLOSURE OF INVENTION

According to a first invention of a control device for a mobile body inaccordance with the present invention, a control device for a mobilebody that controls the operation of the mobile body comprises a desiredmotion determining means for determining a desired motion of the mobilebody, such as a mobile robot having three or more ground contactportions connected through the intermediary of a plurality of jointssuch that mutual relative heights can be manipulated, and an actualposture inclination detecting means for detecting or estimating theactual posture inclination of a predetermined part, such as a base bodyof the mobile body, the operation of the mobile body being controlled tomake the motion of the mobile body follow a desired motion, and furthercomprises:

node operation controlling means that categorizes the ground contactportions in a tree structure manner such that all of the ground contactportions become leaf nodes, and an intermediate node exists between theleaf nodes and a root node having all the leaf nodes as descendantnodes,

determines, on each of C-th nodes, which is a node having a plurality ofground contact portions as descendant nodes, the correction amounts ofdesired relative heights of a plurality of ground contact portionsbelonging to the C-th node for each of the C-th nodes such that aposture inclination error is approximated to zero on the basis of atleast the posture inclination error, which is the difference between theactual posture inclination and a desired posture inclination of thepredetermined part in the desired motion, determines, on the basis of acorrection amount obtained by combining the determined correctionamounts of all the C-th nodes, a corrected desired motion by adding atleast a first correction of the desired relative heights of theplurality of ground contact portions belonging to each C-th node to thedesired motion, and operates the joints such that the corrected desiredmotion that has been determined is satisfied.

In the present invention, regarding each node having child nodes,“ground contact portions belonging to the node” means, more accurately,“belonging as descendant nodes to the node.” This applies not only tothe first invention but applies any one of the inventions of the presentapplication.

According to the first invention, three or more ground contact portionsprovided in a mobile body are sorted into a tree structure, and thecorrection amounts (in other words, manipulated variables) of desiredrelative heights of a plurality of ground contact portions belonging tothe C-th node are determined for each C-th node so as to approximate aposture inclination error to zero on each of the C-th nodes having childnodes. Then, based on a correction amount obtained by combining thosecorrection amounts of all the C-th nodes, a corrected desired motion isdetermined by adding a first correction of the desired relative heightsof the plurality of ground contact portions belonging to each C-th nodeto the desired motion. Then, the joints of the mobile body are operatedsuch that the determined corrected desired motion is satisfied. Thismakes it possible to adjust the relative heights of the ground contactportions so as to approximate the posture inclination error to zerowhile accomplishing mutual balance of the floor reaction forces of theplurality of ground contact portions, as compared with a case where theheights of individual ground contact portions are independently(separately) adjusted. As a result, even if a floor with which a mobilebody is to be in contact has a locally unknown tilt, projection ordepression, an influence thereof will be minimized, permitting enhancedstability of a posture of a robot to be achieved.

In the first invention described above, preferably, each node in thetree structure has a set weight, and regarding a B-th, which is eachnode in the tree structure, if the B-th node is the leaf node, then theheight of a ground contact portion, which is the leaf node, is definedas the height of the B-th node, and if the B-th node has child nodes,then a weighted mean value, which uses the weight, of the heights of allchild nodes of the B-th node is defined as the height of the B-th node,and

if the node operation controlling means determines the corrected desiredmotion for only one arbitrary node among the C-th nodes by adding afirst correction of the desired relative heights of the plurality ofground contact portions belonging to that one node to the desiredmotion, then the desired height of that one node in the correcteddesired motion is preferably maintained at a desired height in thedesired motion (a second invention).

According to the second invention, the desired height of each C-th nodetakes the weighted mean value of the desired heights of the child nodesof the C-th node (the desired heights are determined on the basis of thedesired heights of the ground contact portions belonging to the C-thnode in the aforesaid desired motion). If the corrected desired motionis determined for only one arbitrary node among the C-th nodes by addinga first correction of the desired relative heights of the plurality ofground contact portions belonging to that one node to the desiredmotion, then the first correction is added to a desired motion such thatthe aforesaid weighted mean value of the child nodes of the aforesaidone node will be the same in a corrected desired motion and a desiredmotion.

As a result, for each C-th node, the desired relative heights of theplurality of ground contact portions belonging to each C-th node can becorrected while achieving mutual balance among the floor reaction forcesof the plurality of ground contact portions belonging to each C-th node.This eventually permits further improved stability of a posture of therobot.

In a third invention of the control device for a mobile body inaccordance with the present invention, a control device for a mobilebody that controls the operation of the mobile body is equipped withdesired motion determining means for determining a desired motion of themobile body, such as a mobile robot having three or more ground contactportions connected through the intermediary of a plurality of jointssuch that mutual relative heights can be adjusted, and actual postureinclination detecting means for detecting or estimating the actualposture inclination of a predetermined part, such as the base body ofthe mobile body, the operation of the mobile body being controlled tomake the motion of the mobile body follow a desired motion,

when the ground contact portions are sorted into a tree structure suchthat each of the ground contact portions provides a leaf node, and anintermediate node exists between the leaf node and a root node havingall leaf nodes as descendant nodes thereof, a weight is set for eachnode in the tree structure,

comprising a node operation controlling means that determines acorrected desired motion obtained by adding at least a first correctionof desired relative heights of a plurality of ground contact portionsbelonging to the C-th nodes to the desired motion so as to approximate aposture inclination error to zero on the basis of the postureinclination error, which is the difference between at least the actualposture inclination and a desired posture inclination of thepredetermined part in the desired motion, and then operates the jointssuch that the corrected desired motion that has been determined issatisfied,

wherein, regarding a B-th node, which is each node in the treestructure, if the B-th node is the leaf node, then the height of theground contact portion, which is the leaf node, is defined as the heightof the B-th node, and if the B-th node has a child node, then a weightedmean value, which uses the weight, of the heights of all child nodes ofthe B-th node, is defined as the height of the B-th node, and if thenode operation controlling means determines the desired motion for onlythe C-th node by adding a first correction of the desired relativeheights of the plurality of ground contact portions belonging to theC-th node to the desired motion, then the desired height of the C-thnode in the corrected desired motion is maintained at a desired heightin the desired motion.

According to the third invention, the desired height of thepredetermined C-th node takes the weighted mean value of the desiredheights of the child nodes of the C-th node (this is determined on thebasis of the desired heights of the ground contact portions belonging tothe C-th node in the desired motion). If the aforesaid corrected desiredmotion is determined for only the C-th node by adding a first correctionof the desired relative heights of a plurality of ground contactportions belonging to the C-th node to the desired motion, then thefirst correction is added to a desired motion such that the aforesaidweighted mean value of the child nodes of the C-th node will be the samein a corrected desired motion and a desired motion.

As a result, according to the third invention, while achieving mutualbalance among the floor reaction forces of the plurality of groundcontact portions belonging to the C-th node, the relative heights of theground contact portions can be adjusted so as to approximate the postureinclination error to zero. Thus, as with the first invention or thesecond invention, even if a floor with which a mobile body is to be incontact locally has an unknown inclination, projection or depression,the influences thereof will be minimized, permitting improved stabilityof a posture of a robot to be achieved.

In the second invention and the third invention described above,regarding each node having child nodes in the tree structure, theweights of all child nodes of each node are preferably set such that thetotal sum of the weights of all the child nodes will be one (a fourthinvention).

Preferably, the second invention and the third invention described aboveis equipped with a means for variably setting the weights of the nodes,and if the leaf nodes corresponding to ground contact portions that arenot in contact with the ground are defined as D-th nodes, then the meansfor setting the weights sets the weight of each D-node or the weight ofat least one of ancestor nodes of the D-th node to zero (a fifthinvention).

The fifth invention makes it possible to prevent the height of theground contact portion not in contact with the ground, which cannotcontribute to bringing the posture inclination error to zero, from beingwastefully corrected. The fifth invention may be combined with thefourth invention.

In the first invention and the second invention, if the ground contactportions are connected to the base body of the mobile body in such amanner that they are movable with respect to the base body, then thenode operation controlling means, when determining the corrected desiredmotion, preferably determines the corrected desired motion by furtheradding to the desired motion a correction of a desired posture of thebase body to restrain slippage, such as twist, of the ground contactportions belonging to the C-th node on a floor surface (a sixthinvention). This applies also to the third invention described above (aseventh invention).

If there are many ground contact portions connected to the base body ofa mobile body, the degrees of freedom of joints of the mobile body willbe excessively low, causing the corrections of the relative heights ofthe ground contact portions to be subject to mechanical restrictions. Ifthe joints are forcibly operated to satisfy determined desired relativeheights, then some ground contact portions may incur slippage, includingtwist. In such a situation wherein a slippage occurs, there is a dangerin that the floor reaction forces of the ground contact portions becomeinappropriate and the stability of a posture of the robot is impaired.In such a case, as in the sixth invention and the seventh invention,rather than simply correcting the desired relative heights of the groundcontact portions belonging to the C-th node, a correction of a desiredposture of the base body is further added to a desired motion so as todetermine the corrected desired motion, thus making it possible torestrain slippage, including twist. Hence, the ground contact portionswill not be subjected to undue floor reaction forces, making it possibleto secure stability of a posture of the mobile body by setting the floorreaction forces to be appropriate.

In the sixth invention and the seventh invention, more preferably, thenode operation controlling means determines the corrected desired motionsuch that the horizontal position of the overall center-of-gravity ofthe mobile body in the corrected desired motion or the horizontalposition of a predetermined representative point of the base body of themobile body is substantially the same as the horizontal position in thedesired motion (an eighth invention). The eighth invention makes itpossible to prevent the overall center-of-gravity of the mobile bodyfrom being considerably shifted in the horizontal direction relative toa proper position in a desired gait when a corrected desired motion isdetermined by adding a correction of a desired posture of the base bodyto a desired motion. Hence, the relative heights of the ground contactportions of the C-th node and the posture of the base body can becorrected while maintaining proper stability of the mobile body.

The sixth invention is suited to a case where the mobile body is a robotequipped with a plurality of link mechanisms extended from its basebody, and at least one link mechanism out of the plurality of linkmechanisms has a joint provided at an intermediate portion between atleast its distal portion and the end portion adjacent to the base body,the intermediate portion being the ground contact portion (a ninthinvention).

The seventh invention is suited to a case where the mobile body is arobot equipped with a plurality of link mechanisms extended from itsbase body, and at least one link mechanism out of the plurality of linkmechanisms has a joint provided at an intermediate portion between atleast its distal portion and the end portion adjacent to the base body,the intermediate portion being the ground contact portion belonging tothe C-th node (a tenth invention).

More specifically, a robot with a plurality of link mechanisms extendedfrom its base body usually contacts the ground through the distalportions of the link mechanisms; therefore, an attempt to make the robotcontact the ground at intermediate portions of the link mechanisms(e.g., an attempt to make a human type mobile robot contact the groundat an intermediate portion (knee) of a leg thereof) leads to theabove-mentioned slippage, such as twist, due to an insufficient degreeof freedom of a joint in some cases. Hence, the sixth invention or theseventh invention is suited to a case where a mobile body is constructedas described in the ninth invention or the tenth invention.

In these ninth invention and tenth invention, the intermediate portionis preferably provided with an elastic member that resiliently deformswhen it comes in contact with the ground (an eleventh invention).

With this arrangement, the capability of adjusting a floor reactionforce by correcting the relative heights of the intermediate portions isenhanced, and the stability of a posture of a mobile body (robot) can beeasily secured.

Further, the sixth invention is suited also to a case where the mobilebody is equipped with a plurality of link mechanisms which are extendedfrom its base body and each of which has one or more joints, and atleast one link mechanism among the plurality of link mechanisms and thebase body are provided with the ground contact portions (a twelfthinvention).

Further, the seventh invention is suited also to a case where the mobilebody is equipped with a plurality of link mechanisms which are extendedfrom its base body and each of which has one or more joints, at leastone link mechanism among the plurality of link mechanisms and the basebody are provided with the ground contact portions, and ground contactportions belonging to the C-th node include at least the ground contactportion of the base body (a thirteenth invention).

For instance, when making a robot that has one or more legs (linkmechanisms) extended from a base body (a body or the like) sit down, thebase body is to come in contact with the ground. And in such a case, aninsufficient degree of freedom of joints occasionally results, causingthe aforementioned slippage, such as twist, to take place. Thus, thesixth invention or the seventh invention is suited also to a case wherea mobile body is constructed as described in the twelfth invention orthe thirteenth invention.

In these twelfth invention and the thirteenth invention, preferably, atleast one ground contact portion of the base body is provided with anelastic member that resiliently deforms when it comes in contact withthe ground (a fourteenth invention).

With this arrangement, the capability of adjusting a floor reactionforce by correcting the relative heights of the ground contact portionsin the base body is enhanced, and the stability of a posture of a mobilebody (robot) can be easily secured.

The sixth to the fourteenth inventions described above may be combinedwith the second invention, the fourth invention or the fifth inventiondescribed above.

Further, the first invention and the third invention described above (orthe first to the fourteenth inventions) preferably comprises a means fordetermining a translational force component of a desired floor reactionforce to be applied to ground contact portions belonging to the C-thnode when the joint is operated to satisfy the corrected desired motionif the ground contact portions are connected to the base body throughthe intermediary of connecting mechanisms such that they are movablewith respect to the base body of the mobile body,

wherein the node operation controlling means comprises a means forestimating a deformation amount that occurs at the connecting mechanismsbetween the base body and the ground contact portions belonging to theC-th node and the ground contact portions when the translational forcecomponent of the desired floor reaction force is applied to the groundcontact portions belonging to the C-th node on the basis of at least thetranslational force components of the desired floor reaction forces ofthe ground contact portions belonging to the C-th node, and determinesthe corrected desired motion by further adding a second correction ofthe desired heights of the plurality of ground contact portionsbelonging to the C-th node, which is for canceling the estimateddeformation amount, to the desired motion (a fifteenth invention).

According to the fifteenth invention, the occurrence of a deformation,such as a flexure, of the connecting mechanisms or the ground contactportions caused by a floor reaction force applied when the groundcontact portions come in contact with the ground is taken into account,and the amount of the deformation is estimated on the basis of thetranslational force component of a desired floor reaction force. Then,the second correction of the desired heights of the ground contactportions belonging to the C-th node for canceling the estimateddeformation amount is further added to the desired motion so as todetermine the corrected desired motion. Thus, the fifteenth inventionmakes it possible to apply a floor reaction force that properlyapproximates the posture inclination error to zero to a mobile bodywhile compensating for the influences of the deformations of theconnecting mechanisms or the ground contact portions, allowing thestability of a posture of the mobile body to be further improved.

Preferably, the first invention and the third invention (or the first tothe fifteenth inventions) comprises a floor configuration estimatingmeans for estimating a parameter that specifies the relative heights ofthe ground contact surfaces of at least a plurality of ground contactportions belonging to the C-th node as the floor configuration parameterthat represents a floor configuration,

wherein the node operation controlling means determines the correcteddesired motion by further adding a third correction of the desiredrelative heights of a plurality of the ground contact portions belongingto the C-th node on the basis of an estimated value of a floorconfiguration parameter when determining the corrected desired motion (asixteenth invention).

According to the sixteenth invention, a third correction of the desiredrelative heights of a plurality of the ground contact portions belongingto the C-th node on the basis of an estimated value of a floorconfiguration parameter that specifies at least the relative heights ofthe ground contact surfaces of a plurality of ground contact portionsbelonging to the C-th node is further added to a desired motion;therefore, even if an actual floor configuration differs from the oneassumed in the desired motion, it is possible to make the ground contactsurfaces of the ground contact portions properly come in contact withthe actual floor surface when they are to be in contact with it andconsequently to cause a floor reaction force that approximates theposture inclination error to zero to properly act on a mobile body.Hence, the stability of a posture of the mobile body can be furtherenhanced.

Preferably, the sixteenth invention comprises a floor reaction forcedetecting means for detecting or estimating an actual floor reactionforce that acts on each of the ground contact portions,

wherein the floor configuration estimating means performs estimationwhile sequentially updating the floor configuration parameter on thebasis of a past value of an estimated value of the floor configurationparameter, a difference between the corrected desired motion and thedesired motion, the corrected motion, at least either a detected valueor an estimated value of an actual joint displacement, which is adisplacement amount of each joint of the mobile body, the actual postureinclination, and an actual floor reaction force of each of the groundcontact portions belonging to at least the C-th node (a seventeenthinvention).

Alternatively, the invention preferably comprises a floor reaction forcedetecting means for detecting or estimating an actual floor reactionforce that acts on each of the ground contact portions, and

a means for sequentially determining at least an actual node floorreaction force, which is an actual value of the node floor reactionforce of each child node of the C-th node, from the actual floorreaction force of each ground contact portion belonging to the C-th nodeif, regarding A-th nodes, which are the nodes in the tree structure, theA-th nodes are the leaf nodes, then the floor reaction forces acting onthe ground contact portions, which are the leaf nodes, are defined asthe node floor reaction forces of the A-th nodes, and if the A-th nodeshave child nodes, then the resultant force of the node floor reactionforces of all the child nodes of the A-th nodes is defined as the nodefloor reaction force of the A-th nodes,

wherein the floor configuration estimating means performs estimationwhile sequentially updating the floor configuration parameter on thebasis of the past value of an estimated value of the floor configurationparameter, the difference between the corrected desired motion and thedesired motion, the corrected motion, at least either a detected valueor an estimated value of an actual joint displacement, which is adisplacement amount of each joint of the mobile body, the actual postureinclination, and a relative relationship among the actual node floorreaction forces of the child nodes of at least the C-th node (aneighteenth invention).

Alternatively, if the ground contact portions are connected to the basebody through the intermediary of connecting mechanisms such that theyare movable with respect to the base body of the mobile body,

the invention preferably comprises a deformation amount detecting meansfor detecting or estimating the amounts of deformations that occur atthe connecting mechanisms between the base body and the ground contactportions and at the ground contact portions,

wherein the floor configuration estimating means performs estimationwhile sequentially updating the floor configuration parameter on thebasis of the past value of an estimated value of the floor configurationparameter, a difference between the corrected desired motion and thedesired motion, the corrected motion, at least either a detected valueor an estimated value of an actual joint displacement, which is adisplacement amount of each joint of the mobile body, the actual postureinclination, and the deformation amount associated with each groundcontact portion belonging to at least the C-th node (a nineteenthinvention).

According to the seventeenth invention, the eighteenth invention, andthe nineteenth invention, floor configuration parameters can be properlyestimated while sequentially updating them in real time. Supplementally,in the nineteenth invention, it is not necessarily required that all theconnecting mechanisms be provided with joints.

Further, the nineteenth invention comprises, for example, a floorreaction force detecting means for detecting or estimating an actualfloor reaction force acting on each of the ground contact portions,wherein the deformation amount detecting means estimates the deformationamount on the basis of the actual floor reaction force (a twentiethinvention).

By detecting or estimating an actual floor reaction force as describedabove and using it, the deformation amounts of the connecting mechanismsor the ground contact portions can be properly estimated.

In the sixteenth invention (or the sixteenth to the twentiethinventions), each node in the tree structure has a set weight, andregarding a B-th node, which is a node in the tree structure, if theB-th node is the leaf node, then the height of the ground contactsurface of the ground contact portion, which is the leaf node, isdefined as the height of the ground contact surface of the B-th node,and if the B-th node has child nodes, then a weighted mean value, whichuses the weight, of the heights of all child nodes of the B-th node isdefined as the height of the ground contact portion of the B-th node,and when these definitions apply,

the floor configuration parameter estimated by the floor configurationestimating means is preferably a parameter that specifies a relativerelationship of the heights of the ground contact surfaces of aplurality of child nodes of the C-th node (a twenty-first invention).

According to the twenty-first invention, defining the height of theground contact surface of the B-th node having child nodes as theweighted mean value of the child nodes, as described above, makes itpossible to determine a floor configuration parameter related to theconfiguration of a floor with which a plurality of ground contactportions belonging to the C-th node comes in contact by the relativerelationship among the heights of the ground contact surfaces of theplurality of child nodes of the C-th node. Hence, floor configurationparameters can be estimated without grasping the absolute heights of theground contact portions. This permits easier processing for estimating afloor configuration.

In the twenty-first invention, if at least one of the child nodes of theC-th node is about to float, the floor configuration estimating meanspreferably estimates the floor configuration parameter that specifiesthe relative relationship of the heights of the ground contact surfacesof the plurality of child nodes while retaining, at a fixed value, therelative height of the ground contact surface of the child node that isabout to float (a twenty-second invention).

More specifically, the state in which at least one of the child nodes ofthe C-th node is about to float means that the ground contact surface ofthe child node has left or is about to leave an actual floor surface;therefore, the height of the ground contact surface of the child nodedoes not reflect the height of the actual floor surface. Hence,according to the twenty-second invention, in such a state, the floorconfiguration parameter that specifies the relative relationship of theheights of the ground contact surfaces of the plurality of child nodesis estimated while holding, at a fixed value, the relative height of theground contact surface of the child node that is about to float. Thismakes it possible to prevent a floor configuration parameter fromdiverging into a value that does not reflect an actual floor surfaceconfiguration, and to estimate a floor configuration parameter thatreflects an actual floor surface configuration. Consequently, it ispossible to prevent an inappropriate floor configuration parameter frombeing used to correct the relative heights of ground contact portions,which would impair the stability of a posture of a mobile body.

Further, in the sixteenth invention (or the sixteenth to thetwenty-second inventions) described above, the floor configurationestimating means preferably estimates the floor configuration parameterby using a low-pass filter to alleviate fluctuation in the floorconfiguration parameter (a twenty-third invention). Using a low-passfilter like this makes it possible to prevent a floor configurationparameter from frequently varying due to a vibration or the like when amobile body moves.

Further, in the sixteenth invention (or the sixteenth to thetwenty-third inventions), a weight for estimating a floor configurationis set on each node of the tree structure, and

if processing in which, when a predetermined type of state amount, suchas a height or a floor reaction force, is associated with each leaf nodein the tree structure, the state amount of each node having child nodesis defined as a weighted mean value, which uses the weight, of the stateamounts of all child nodes of the node, a value obtained by subtractinga state amount of a parent node of each node from the state amount ofthe node is determined as a node relative state amount of the node onthe node except a root node, and zero is determined as a relative stateamount of the root node, is defined as the processing for relativelyhierarchizing the predetermined type of state amount on each node,

if a node relative floor reaction force Fn_rel(n=1,2, . . . )hierarchically relativized on each node is determined on the basis of afloor reaction force Fn(n=1,2, . . . ) acting on each of the groundcontact portions, which are the leaf nodes, and when a vector(Fa1_rel,Fa2_rel, . . . ,Far_rel) having the node relative floorreaction forces of all child nodes aj (j=1,2, . . . ,r. r denotes thetotal number of the child nodes of an n-th node) of an n-th node, whichis an arbitrary node having child nodes, is represented by a linearlinkage of a plurality of predetermined mutually independent vectorsR(j) (j=1,2, . . . ,r-1) that are all orthogonal to a vector (Wa1,Wa2, .. . ,War) having the weights of all child nodes of the n-th node as itselements, a vector having a coefficient of the linear linkage as itselement is defined as a node expansion floor reaction force momentMn_exp of the n-th node, and

if a node relative height Zn_rel(n=1,2, . . . ) hierarchicallyrelativized on each node is determined on the basis of a heightZn(n=1,2, . . . ) of the ground contact surface of each of the groundcontact portions, which are the leaf nodes, and when a vector (Za1_rel,Za2_rel, . . . ,Zar_rel) having the node relative heights of all childnodes aj (j=1,2, . . . ,r. r denotes the total number of the child nodesof an n-th node) of the n-th node as its elements is expressed by alinear linkage of the plurality of the predetermined mutuallyindependent vectors R(j) (j=1,2, . . . ,r-1), a vector having acoefficient of the linear linkage as its element is defined as a nodeexpansion inclination angle θn of the n-th node,

then, the floor configuration estimating means may estimate the floorconfiguration parameter by using at least the node expansion floorreaction force moment Mn_exp(n=C) or the node expansion inclinationangle θn (n=C) of at least the C-th node (a twenty-fourth invention).

Using the node expansion floor reaction force moment or the nodeexpansion inclination angle defined as described above makes it possibleto properly estimate a floor configuration parameter that represents afloor configuration even if there are four or more ground contactportions belonging to the C-th node.

In the twenty-fourth invention, regarding a B-th node, which is a nodein the tree structure, if the B-th node is the leaf node, then theheight of the ground contact surface of the ground contact portion,which is the leaf node, is defined as the height of the ground contactsurface of the B-th node, and if the B-th node has child nodes, then aweighted mean value, which uses the weight, of the heights of the groundcontact surfaces of all child nodes of the B-th node, is defined as theheight of the ground contact surface of the B-th node, then

the floor configuration parameter preferably includes a parameter thatuses the node expansion inclination angle to indicate the relativeheight of the ground contact surface of each child node of the C-th node(a twenty-fifth invention).

With this arrangement, a floor configuration parameter can be properlyestimated by using the node expansion inclination angle even if thereare four or more ground contact portions belonging to the C-th node.

Further, more specifically, the twenty-fourth invention comprises afloor reaction force detecting means for detecting or estimating anactual floor reaction force acting on each of the ground contactportions,

wherein the node operation controlling means comprises a means forsequentially determining the node expansion moment on the basis of atleast an actual floor reaction force of each of the ground contactportions, and a means for sequentially determining the node expansioninclination angle by multiplying the determined node expansion moment bya predetermined matrix, and estimates a new floor configurationparameter on the basis of the determined node expansion inclinationangle and the past value of an estimated value of the floorconfiguration parameter (a twenty-sixth invention).

According to the twenty-sixth invention, the node expansion moment issequentially determined on the basis of at least the actual floorreaction force of each of the ground contact portions and the nodeexpansion inclination angle is sequentially determined by multiplyingthe determined node expansion moment by a predetermined matrix;therefore, a new floor reaction force parameter can be determined whilesequentially updating it on the basis of the determined node expansioninclination angle and the past value of an estimated value of the floorconfiguration parameter.

In the first to the twenty-sixth inventions described above, “floorreaction force” is to include a reaction force received from an objectother than a floor with which a mobile body comes in contact rather thansimply meaning a reaction force acting on the mobile body from a floor(or the ground), as in its original meaning. Further, the ground contactportion is to include not only a portion that comes in contact with afloor (or the ground) but also a portion of the mobile body that comesin contact with an object.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is an overall external view of a robot in accordance with a firstembodiment and a second embodiment of the present invention,

FIG. 2 is a block diagram showing the functional construction of acontrol device of the first embodiment,

FIGS. 3( a) to (c) and FIGS. 4( a) to (c) are diagrams for explaining anoperation of the robot (four-legged robot) of the first embodiment,

FIGS. 5( a) to (c) are graphs showing setting examples of weights in thefirst embodiment, and

FIG. 5( d) is a graph showing a setting example of a ZMP (desired totalfloor reaction force central point) in the first embodiment.

FIG. 6 is a diagram showing the hierarchical structure of nodes in thefirst embodiment,

FIG. 7 is a diagram showing a relationship between desired node floorreaction force central points and weights,

FIG. 8 is a diagram showing a relationship between desired node floorreaction forces and weights, and

FIG. 9 is a flowchart showing a main routine processing of the controldevice of the first embodiment.

FIG. 10 to FIG. 14 are diagrams illustrating desired node floor reactionforce translational force components, actual node floor reaction forcetranslational force components, actual node floor reaction forcemoments, compensating total floor reaction force moments, and nodecompensating floor reaction force moments, respectively, of a robot(four-legged robot) of the first embodiment.

FIG. 15 and FIG. 16 are diagrams for explaining position corrections bynode compensating angles (compliance operation) in the first embodiment,

FIG. 17 to FIG. 21 are block diagrams respectively showing thefunctional constructions of a hierarchical compliance operationdeterminer, a compensating total floor reaction force momentdistributor, a θ1423 determiner (compensating angle determiner), a θ14determiner (compensating angle determiner), and a mechanism deformationcompensation amount calculator in the first embodiment,

FIG. 22 is a flowchart showing the processing for determiningcompensating angles, and

FIG. 23 is a diagram for explaining another example of a hierarchicalstructure related to the robot according to the first embodiment.

FIG. 24 is a diagram for explaining the hierarchical structure of arobot (six-legged robot) according to a second embodiment,

FIG. 25 is a block diagram showing the functional construction of ahierarchical compliance operation determiner in the second embodiment,

FIG. 26 to FIG. 28 are diagrams respectively illustrating thetranslational force components of desired node floor reaction forces,the translational force components of actual node floor reaction forces,and actual node floor reaction force moments of the robot (six-leggedrobot) according to the second embodiment,

FIGS. 29( a) and (b) are diagrams for explaining the technique forsetting a node compensating floor reaction force moment in the secondembodiment, and

FIG. 30 and FIG. 31 are diagrams for explaining a position correction bya node compensating angle (compliance operation) in the secondembodiment.

FIG. 32 and FIG. 33 are block diagrams respectively showing thefunctions of a θ145236 determiner (compensating angle determiner) and aθ145 determiner (compensating angle determiner) in the secondembodiment, and

FIG. 34 is a diagram for explaining another example of a hierarchicalstructure related to the robot according to the second embodiment.

FIG. 35 is a block diagram showing the functional construction of ahierarchical compliance operation determiner in a third embodiment,

FIG. 36 is a flowchart showing a main routine processing of a controldevice in the third embodiment,

FIG. 37 to FIG. 39 are diagrams for explaining concepts and terms in thethird embodiment,

FIG. 40 is a block diagram showing the function of a floor configurationestimator in the third embodiment, and

FIG. 41 is a flowchart showing the processing of a floor height errorestimation processing subroutine in FIG. 40.

FIG. 42 and FIG. 43 are diagrams for explaining the processing forhierarchical relativization in a four-legged robot and a six-leggedrobot, respectively.

FIG. 44 is a diagram showing examples of setting request modes of nodesrelated to estimating a floor configuration, and

FIG. 45 to FIG. 55 are flowcharts showing the processing for estimatinga floor configuration.

FIG. 56 is a block diagram showing the functions of a floorconfiguration estimator in a fourth embodiment, and

FIG. 57 is a block diagram showing the functions of a floorconfiguration estimator in a fifth embodiment.

FIG. 58 and FIG. 59 are diagrams showing a state wherein a robot(two-legged mobile robot) in a sixth embodiment is on its knee incontact with a floor,

FIG. 60 is a diagram showing the construction of a floor reaction forcesensor of a knee of the robot in the sixth embodiment,

FIG. 61 is a diagram showing a hierarchical structure of nodes in thesixth embodiment, and

FIG. 62 is a block diagram showing the functional construction of ahierarchical compliance operation determiner in the sixth embodiment.

FIGS. 63( a) to (c) are diagrams for explaining a technique forcorrecting the posture of the body of the robot in the sixth embodiment,

FIG. 64 is a diagram for explaining a technique for correcting theposition/posture of the body of the robot in the sixth embodiment, and

FIG. 65 is a block diagram showing the functions of an inversekinematics calculator in the sixth embodiment

FIG. 66 is a block diagram showing the functions of an inversekinematics calculator in a seventh embodiment, and

FIG. 67 and FIG. 68 are diagrams for explaining a technique forcorrecting the posture of a robot in the seventh embodiment.

FIG. 69 is a diagram showing the construction of a robot in an eighthembodiment and

FIG. 70 is a diagram showing a hierarchical structure of nodes in theeighth embodiment.

BEST MODE FOR CARRYING OUT THE INVENTION

First, major terms to be used in the explanation of the embodiments ofthe present invention will be explained.

In the technology of the composite-compliance control disclosed inJapanese Unexamined Patent Application Publication No. H10-277969previously proposed by the present applicant, floor reaction forces aresorted for each leg on the assumption that floor reaction forces arereceived at the distal portion (foot) of a leg of a robot. In comparisontherewith, according to the embodiments of the present description,situations are also considered in which floor reaction forces arereceived at portions other than the distal portions of legs, e.g., asituation in which a human type robot is on its knee, sitting on achair, or has its arm in contact with the ground. For this reason, aportion of a legged mobile robot that comes in contact with a floor andreceives a reaction force in a predetermined operation of the robot isreferred to as “the ground contact portion.” In this case, “floor” doesnot simply refers to a floor (or the ground) in a conventional sense,but “floor” also includes an object with which the robot comes incontact and receives a reaction force during its movement, such as achair (a chair on which the robot sits) fixed to the floor (or theground). Thus, if, for example, a two-legged mobile robot sits on achair or the like, then the waist of the robot will correspond to theground contact portion. In a normal travel of a legged mobile robot (inthe walking or the like of the two-legged mobile robot), the distalportion (foot or the like) of each leg will of course correspond to theground contact portion.

To categorize (discriminate) ground contact portions, even if groundcontact portions are distributed such that they are separated at aplurality of locations on the same link (a part corresponding to asingle rigid body) (if a plurality of portions separated from each otheron the same link is in contact with the ground), that is, even if aplurality of ground contact surfaces are not connected on the same link,these will be put together and defined as one ground contact portion.For instance, if the ground contact portion provided with four spikepins comes in contact with the ground through the intermediary of thespike pins, then the individual ground contact surfaces of the fourspike pins will be put together and regarded as one ground contactportion. However, it is not always required to put the ground contactsurfaces together into one.

An n-th ground contact portion is a designation for identifying theground contact portion which has been categorized (discriminated)according to such a rule and to which an identification number n(n=1,2,3, . . . ) has been assigned.

A ground contact portion floor reaction force refers to a floor reactionforce acting on the ground contact portion, and the floor reaction forceacting on an n-th ground contact portion, in particular, is referred toas the n-th ground contact portion floor reaction force. The total floorreaction force is the resultant force of the floor reaction forcesacting on all ground contact portions. The floor reaction force centralpoint is the point of action at which the horizontal component of amoment generated by a floor reaction force at that point reaches zero.

A floor reaction force, such as the ground contact portion floorreaction force or a total floor reaction force, is usually expressed bya set of the point of action of a force and a translational force andmoment applied to that point of action. For the same floor reactionforce, there are a limitless number of sets of point of action,translational force and moment for expressing the floor reaction force.Among them, there is an expression of a floor reaction force that uses,as the point of action, a point at which a moment component (thehorizontal component of a moment) except a component about a verticalaxis (vertical component) reaches zero. The point of action in thisexpression is referred to as a floor reaction force central point in thepresent description. A floor reaction force central point may be definedas a point at which the floor surface parallel component (the componentparallel to a floor surface) of a moment generated by a floor reactionforce at that point reaches zero.

In the present description, “floor surface” may alternatively be avirtual floor surface (a floor surface assumed in a desired gait and itdoes not necessarily coincide with an actual floor surface) described inJapanese Unexamined Patent Application Publication No. H5-318340previously proposed by the present applicant.

Supplementally, the floor reaction force central point of the groundcontact portion is usually set on the ground contact surface thereof(the surface in contact with a floor) if the ground contact portion isin contact with the ground. Meanwhile, the ground contact portion floorreaction force while the ground contact portion is moving in the air isalways zero, so that a moment horizontal component of the ground contactportion floor reaction force will be zero at any point of action. Thismeans that the floor reaction force central point can be arbitrarilyset. However, to smoothly control the operation of a robot, a desiredfloor reaction force central point desirably moves in continuity. Hence,according to the embodiments in the present description, a floorreaction force central point of the ground contact portion floorreaction force is defined as the point of action that moves togetherwith the ground contact portion when the ground contact portion moves inthe air.

According to the embodiments in the present description, in thecompliance control operation in which the position/posture of eachground contact portion are corrected on the basis of at least an actualfloor reaction force (a floor reaction force actually acting on arobot), the compliance control operation is not simply performed on eachof ground contact portions irrelevantly (independently) from each other.The embodiments in the present description are characterized in thatground contact portion are sorted by a tree structure and operations forcorrecting the positions/postures of ground contact portions aredetermined on the basis of at least an actual floor reaction forceacting on each group that has been sorted (the actual floor reactionforces or the resultant force thereof acting on all ground contactportions included in each group). Hereinafter, sorting by the treestructure may be referred to as “hierarchization.”

For the desired values of variables generated by a gait generatingdevice of a legged mobile robot in the embodiments in the presentdescription, “desired” will be attached to the beginning of the names ofvariables. For the values of the variables in an actual legged mobilerobot (actually detected values or estimated values, because true valuesare unknown), “actual” will be attached to the beginning of the names ofvariables. The above “actual floor reaction force” is an examplethereof.

The targets of the total floor reaction forces (the resultant force ofthe actual floor reaction forces acting on all ground contact portionsof the robot) in the compliance control (floor reaction force control)to be discussed later will be referred to as desired total floorreaction forces. The point at which the moment horizontal component of adesired total floor reaction force reaches zero will be referred to as adesired total floor reaction force central point.

The point on a floor surface at which the moment horizontal component ofthe resultant force of an inertial force generated by a motion of adesired gait of a mobile robot (the motion of each portion of the robotin a desired gait) and the gravity acting on the robot becomes zero willbe referred to as a desired ZMP. The motion of a desired gait isdetermined by the time series of the desired position/posture of eachportion of the robot in the desired gait, so that the time series of thedesired position/posture of the portion will be generically referred toas a motion or a desired motion of a desired gait. Supplementally, ifthese time series can be specified, then desired motions may bedescribed by expressions that are different from the desired motionsdefined as described above. For example, a set of the time series of thedesired displacements of joints of a robot and the time series of thedesired position/posture of a particular portion, such as a base body,may be used as a desired motion.

A desired total floor reaction force is usually a total floor reactionforce that dynamically balances with the motion patter of a desired gait(the time-series pattern of a desired motion). Hence, a desired totalfloor reaction force central point usually agrees with a desired ZMP.Thus, in the following explanation, a desired total floor reaction forcecentral point and a desired ZMP will be used without discriminating themin many cases. Exceptionally, however, in the case of controlling arobot explained in, for example, U.S. Pat. No. 3,269,852 previouslyproposed by the present applicant, a desired total floor reaction forcecentral point and a desired ZMP do not necessarily agree with eachother. In the following explanation, the term, desired ZMP, will be usedin some cases, but there will be some places where the term, desiredtotal floor reaction force central point, should be used to be precise.

It may be assumed that, in a desired gait of a mobile robot, the robotis subject to a reaction force (an external force) other than a floorreaction force from an environment. And, the reaction force (theexternal force) other than a floor reaction force may be referred to as,for example, a desired object reaction force, and the definition of adesired ZMP described above may be expanded as follows. The resultantforce of the inertial force generated by a motion pattern of a desiredgait of a robot, the gravity acting on the robot, and a desired objectreaction force may be dynamically determined, and if the momentgenerated at a certain point on a floor surface by the resultant forceis zero except for a component about a vertical axis, then the point maybe defined anew as a desired ZMP. However, if a desired object reactionforce is taken as one form of a floor reaction force, then thedefinition of a desired ZMP may be the same definition previouslydescribed.

Based on the above, the embodiments of the present invention will beexplained in detail below.

FIRST EMBODIMENT

FIG. 1 is an external view of a general multi-legged mobile robot(legged mobile robot) according to first and second embodiments. FIG. 1shows that the robot 1 has six legs (leg bodies), namely, a first leg #1to a sixth leg #6; however, it does not have the fifth leg #5 and thesixth leg #6 in the first embodiment. This means that, in the firstembodiment, the robot 1 is a four-legged robot having four legs (legbodies), the first leg #1 to the fourth leg #4. In FIG. 1, thecomponents of the robot 1 according to the second embodiment are shownby parenthesized reference numerals.

As shown in FIG. 1, in the robot 1 (the four-legged robot) according tothe first embodiment, two legs (the first leg #1 and the third leg #3)are extended from the right side of a body 24, which is the base body ofthe robot 1, such that they are arranged side by side in thelongitudinal direction, and in the same manner, two legs (the second leg#2 and the fourth leg #4) are extended from the left side of the body 24such that they are arranged side by side in the longitudinal direction.A discoid ground contact portion 10 is attached to the distal portion ofeach of the legs #1 to #4 through the intermediary of a spherical joint12, which is a free joint. Hereinafter, when the ground contact portions10 need to be discriminated among the legs #1 to #4, they will bereferred to as the first ground contact portion, the second groundcontact portion, the third ground contact portion, and the fourth groundcontact portion.

Each ground contact portion 10 is engaged with the spherical joint 12such that its central point agrees with the central point of thespherical joint 12 and that a floor reaction force moment (the momentcomponent of a floor reaction force) will not act on the central pointof the ground contact portion 10 (strictly speaking, the spherical joint12). This means that the floor reaction force moment (actual floorreaction force moment) at the central point of the ground contactportion 10 will be always zero.

In the robot 1 shown in the figure, each of the legs #1 to #4 isprovided with joints 14 and 15 at a portion adjacent to the body 24 ofthe robot 1 and at an intermediate portion, respectively, and acompliance mechanism 42 composed of an elastic member, such as a spring,and a six-axis force sensor 34 serving as a floor reaction forcedetecting means for detecting an actual floor reaction force acting onthe ground contact portion 10 are provided in the vicinity of the distalportion of each of the legs #1 to #4 (a portion of the link connectingthe spherical joint 12 and the joint 14, the portion being adjacent tothe spherical joint 12). In the example shown in the figure, the joints14 permit rotation about two axes, while the joints 15 permit rotationabout one axis. The bottom surfaces of the ground contact portions maybe provided with elastic members made of sponge, rubber or the likeserving as compliance mechanisms.

The displacement operation (the rotational operation about each axis) ofeach of the joints 14 and 15 is performed by an actuator, such as anelectric motor, which is not shown. And, an actual joint displacement,which is the actual displacement amount (the angle of rotation abouteach axis), of each of the joints 14 and 15 is detected by a sensor,such as a rotary encoder, which is not shown. The six-axis force sensor34 is capable of detecting the translational forces in the directions ofthree axes and the moments about three axes; however, in the robot 1 inthe first embodiment, no actual floor reaction force moment acts on thecentral points of the ground contact portions 10, as described above.Hence, three-axis force sensors that detect the translational forces inthe directions of three axes or force sensors that detect only thevertical components of the translational forces of actual floor reactionforces may be used in place of the six-axis force sensors 34.

The body 24 incorporates a control device 50 constructed of anelectronic circuit unit that includes a microcomputer, an actuator drivecircuit, etc., a posture sensor 36 for detecting the postures of thebody 24, and a power source not shown (a secondary battery, a capacitor,or the like). The posture sensor 36 is constructed of, for example, anacceleration sensor and a gyro sensor. In the present description,“posture” generally means a spatial orientation (however, “the posture”of the entire robot means an instantaneous value of a motion of therobot). Further, in the present embodiment, the posture sensor 36detects the posture inclinations (inclination angles) in, for example,the pitch directions and roll directions in relation to the verticaldirection, of the postures of the body 24. In other words, the posturesensor 36 functions as an actual posture inclination detecting means fordetecting actual posture inclinations of the body 24.

If the postures (rotational angles) of the body 24 of the robot 1 alsoin the yaw direction are to be controlled, then the rotational angles ofthe body 24 in the yaw direction (in other words, the postures of thebody 24 in the directions of three axes) may be also detected by theposture sensor 36.

FIG. 2 is a block diagram showing the functional construction andoperations of the control device 50. The actual robot 1 shown in FIG. 2is the robot 1 shown in FIG. 1 from which the control device 50, theposture sensor 36, and the six-axis force sensor 34 have been removed.Here, a predetermined coordinate system (XYZ coordinate system) fixed toa floor, in which the approximately front direction of the robot 1 isdefined as X axis, approximately left side direction thereof is definedas Y axis, and the upward direction thereof is defined as Z axis, asshown in FIG. 1, is referred to as “a supporting leg coordinate system”or “a global coordinate system.” Hereinafter, positions, postures,translational forces and moments will be expressed in terms ofcoordinate components of the supporting leg coordinate system (theglobal coordinate system) unless otherwise specified. The origin of thesupporting leg coordinate system (global coordinate system) does nothave to be steadily fixed at a single point; the position of the originwith respect to a floor may be changed while the robot 1 is traveling.For example, the position of the origin of the supporting leg coordinatesystem (the global coordinate system) may be changed each time apredetermined leg of the robot 1 lands.

As shown in FIG. 2, the control device 50 is provided with a gaitgenerating device 100, a desired floor reaction force distributor 102, arobot geometric model (inverse kinematics calculator) 110, ahierarchical compliance operation determiner 114, a displacementcontroller 112, an actual floor reaction force detector 108, a postureerror calculator 103, and a posture stabilization control calculator 104as its functional components (functional means). The following willexplain the overviews of the components of the control device 50.

The gait generating device 100 has a function as a desired gaitdetermining means or a desired motion determining means in the presentinvention, and generates (determines) and outputs a desired gait thatspecifies an operation of the robot 1. In the present embodiment, adesired gait is formed of the trajectory of a desired motion of therobot (the time series of a desired position/posture of each portion ofthe robot) and the trajectory of a desired floor reaction force (thetime series of a set of the desired position of the action point of areaction force received by the robot from a floor and the desired valuesof a translational force and moment applied to the action point). In thepresent description, “trajectory” means a time-series pattern (temporalchange pattern).

The trajectory of a desired motion output by the gait generating device100 is constructed of a desired ground contact portion trajectory, whichis the trajectory of the desired values of the position and the postureof each ground contact portion 10 of the robot 1, and a desired bodyposition/posture trajectory, which is the trajectory of the desiredvalues of the position and the posture of the body 24 of the robot 1. Agait generating device in a robot equipped with joints related to armsor a head, as in a sixth embodiment to be discussed later, determinesand outputs the desired position/posture trajectories of the arms or thehead as constituents of desired motions.

Further, in the present embodiment, the trajectory of a desired floorreaction force output by the gait generating device 100 is formed of adesired total floor reaction force central point trajectory, which isthe trajectory of a desired position of the total floor reaction forcecentral point of the robot 1, and a desired total floor reaction forcetrajectory, which is the trajectory of a desired value of a total floorreaction force that uses the above desired total floor reaction forcecentral point as an action point. In the present embodiment, a desiredtotal floor reaction force central point trajectory is regarded asidentical to a desired ZMP trajectory, which is the trajectory of adesired position of ZMP.

The position of each ground contact portion 10 is the position of acertain representative point of the ground contact portion 10, and therepresentative point is, for example, the projected point obtained byprojecting, in the vertical direction, the central point of each groundcontact portion 10 (the central point of the spherical joint 12) ontothe ground contact surface (bottom surface) of the ground contactportion 10, or the central point of the ground contact portion 10 (thecentral point of the spherical joint 12). Hereinafter, the position ofthe representative point of each ground contact portion 10 will bereferred to simply as the ground contact portion position. Thetrajectory of a desired value of the ground contact portion position (adesired ground contact portion position trajectory) is included in theaforesaid desired ground contact portion trajectory determined by thegait generating device 100.

Here, in the robot 1 of the present embodiment, the ground contactportions 10 are engaged with the spherical joints 12, which are freejoints, so that the postures of the ground contact portions 10 cannot becontrolled. Therefore, according to the present embodiment, the gaitgenerating device 100 does not actually generate the trajectories ofdesired postures of the ground contact portions 10 (does not have togenerate them). In the present embodiment, therefore, the desired groundcontact portion trajectory means the same as a desired ground contactportion position trajectory.

However, if the ground contact portions are attached to the distalportions of the legs such that their postures can be controlled (if theground contact portions are attached to the distal portions of the legsthrough joints that can be operated by actuators), as in the robot ofthe sixth embodiment to be discussed later, then the trajectories ofdesired postures of the ground contact portions should be included inthe desired ground contact portion trajectories. In the presentdescription, the term “ground contact portion position/posture” will befrequently used to generally consider the aforesaid case, the termsubstantially meaning “ground contact portion position” in the presentembodiment.

Referring to FIG. 3( a) to FIG. 3( c) and FIG. 4( a) to FIG. 4( c), adesired ground contact portion trajectory (a desired ground contactportion position trajectory) and a desired total floor reaction forcecentral point trajectory will be explained more specifically. Accordingto the first embodiment, a travel of the robot 1 is accomplished bycarrying out the motions of the legs #1 to #4 by causing a pair of legs,which are to be free legs, to leave a floor and move in the air and thenland at a desired position by repeating, in order, a period during whichthe pair of the first leg #1 and the fourth leg #4 out of the legs #1 to#4 is a pair of supporting leg, while the pair of the second leg #2 andthe third leg #3 is a pair of free legs, a period during which all legs#1 to #4 are supporting legs, and a period during which the pair of thefirst leg #1 and the fourth leg #4 is a pair of free legs, while thepair of the second leg #2 and the third leg #3 is a pair of supportinglegs. A supporting leg is a leg that is in contact with the ground tosupport the self-weight of the robot 1 (a leg to be subject to a floorreaction force that is not zero), and a free leg is a non-supportingleg.

FIG. 3( a) to FIG. 3( c) and FIG. 4( a) to FIG. 4( c) show the desiredground contact portion positions (more specifically, the positions on ahorizontal plane (XY plane)) of the distal ends of the legs #1 to #4during the aforesaid travel of the robot 1 in time series in sequence(in the sequence of time t1 to t6). The triangles corresponding toreference characters Q1 to Q4 in these figures indicate the desiredground contact portion positions of the first to the fourth groundcontact portions 10, respectively (the positions of the aforesaidrepresentative points of the first to the fourth ground contact portions10 on a horizontal plane (XY plane)).

Supplementally, to be precise, the triangles with the referencecharacters Q1 to Q4 in FIG. 3 and FIG. 4 indicate the positions ofdesired node floor reaction force central points (desired ground contactportion floor reaction force central points) to be discussed later,which are related to the ground contact portions 10. In the presentembodiment, however, the representative points of the ground contactportions 10 are set as described above, so that the desired groundcontact portion positions (the desired positions of the representativepoints) of the first to the fourth ground contact portions 10 agree withthe positions of the desired node floor reaction force central points Qn(n=1,2,3,4), which will be discussed later, or have a certain positionalrelationship with the positions of the respective desired node floorreaction force central points Qn (n=1,2,3,4) (the positions on thehorizontal plane (XY plane) agree). For this reason, the trianglescorresponding to the reference characters Q1, Q2, Q3 and Q4 in FIG. 3and FIG. 4 denote the desired node floor reaction force central points,which will be discussed later, of the first to the fourth ground contactportions 10, respectively, and also denote the positions of the first tothe fourth ground contact portions 10, respectively.

FIG. 3( a) shows the desired ground contact portion positions of theground contact portions 10 at the instant (time t1) when the pair of thefirst leg #1 and the fourth leg #4 as free legs are landed, the pair ofthe second leg #2 and the third leg #3 being supporting legs, FIG. 3( b)shows the desired ground contact portion positions at time t2 in a statewherein all the legs #1 to #4 are supporting legs, and FIG. 3( c) showsthe desired ground contact portion positions at time t3 immediatelybefore the pair of the second leg #2 and the third leg #3 as free legsis separated from a floor (lifted into the air), the pair of the firstleg #1 and the fourth leg #4 being supporting legs. Further, FIG. 4( a)shows the desired ground contact portion positions at time t4 in a statewherein the pair of the second leg #2 and the third leg #3 as free legsis lifted into the air, the pair of the first leg #1 and the fourth leg#4 being supporting legs, FIG. 4( b) shows the desired ground contactportion positions at the instant (time t5) when the pair of the secondleg #2 and the third leg #3 as free legs are landed, the pair of thefirst leg #1 and the fourth leg #4 being supporting legs, and FIG. 4( c)shows the desired ground contact portion positions at time t6immediately before the pair of the first leg #1 and the fourth leg #4 asfree legs is separated from a floor, the pair of the second leg #2 andthe third leg #3 being supporting legs. Incidentally, in FIG. 4( a), thedesired ground contact portion positions of the second leg #2 and thethird leg #3, which are free legs, are indicated by dashed-linetriangles. Supplementally, the trajectory of the positions of the groundcontact portions 10 of free legs in the vertical direction (the Z-axisdirection) are determined such that they rise from a floor surface to apredetermined height and then lower to land again.

Point P in these FIG. 3 and FIG. 4 indicate desired total floor reactionforce central points (desired ZMPs). The desired total floor reactionforce central point trajectories are determined such that theycontinuously move while existing in a range in which ZMPs may exist (aregion on a floor surface corresponding to a so-called supportingpolygon) at positions not excessively close to the boundaries of therange (e.g., at an approximately central position of the range wherein aZMP may exist). In the first embodiment, when two legs #1 and #4 or #2and #3 are supporting legs (refer to FIGS. 3( a) and (c) and FIGS. 4( a)to (c)), a desired total floor reaction force central point is set on asegment connecting the representative points of the ground contactportions 10 and 10 of those legs such that it is not excessively closeto an end point of the segment. When all legs #1 to #4 are supportinglegs (refer to FIG. 3( b)), a desired total floor reaction force centralpoint is set within a polygon having the representative points of allground contact portions 10 as its apex angles such that it is notexcessively close to a boundary of the polygon. FIG. 5( d) is a graphillustrating a trajectory of a component ZMPx in the X-axis direction(in the direction in which the robot 1 advances) of the position of thedesired total floor reaction force central point (desired ZMP)determined as described above. FIGS. 5( a) to (c) are graphsillustrating setting examples of weight, which will be discussed later.

Further, a desired body position/posture trajectory determined by thegait generating device 100 is determined using a dynamic model or thelike of the robot 1 such that the horizontal component of a momentgenerated about a desired ZMP by the resultant force of the inertialforce generated by a desired motion of the robot 1 and the gravityacting on the robot 1 becomes zero. Incidentally, “the body position” isthe position of a certain representative point of the body 24.

A desired total floor reaction force determined by the gait generatingdevice 100 is constructed of the desired values of the translationalforce and the moment acting on a desired total floor reaction forcecentral point, and it is determined such that it balances with theresultant force of the inertial force generated by a desired motion ofthe robot 1 and the gravity acting on the robot 1 at a desired totalfloor reaction force central point in the present embodiment. The momenthorizontal component of the desired total floor reaction force about thedesired total floor reaction force central point (desired ZMP) is zero.Supplementally, it is not necessary to determine all components of thetranslational force and the moment acting on the desired total floorreaction force central point as desired values. For instance, if theposture or the floor reaction force of the robot 1 about the verticalaxis is not controlled, then it is not necessary to determine thecomponent of the moment of the desired total floor reaction force aboutthe vertical axis (the component in the Z-axis direction).

The desired ground contact portion trajectory determined by the gaitgenerating device 100 (the desired ground contact portion positiontrajectory) is corrected by a hierarchical compliance operationdeterminer 114, which will be discussed later.

The desired floor reaction force distributor 102 groups (that is,hierarchizes) the first to the fourth ground contact portions 10 of therobot 1 into a tree structure, and associates the nodes of the treestructure with each of the hierarchized groups. Hence, in the followingexplanation, the nodes may be expressed by replacing them by the groups.Each node is a group constructed of one or more ground contact portions10.

In the first embodiment, the ground contact portions 10 arehierarchized, as shown in FIG. 6. More specifically, the n-th groundcontact portion 10 (n=1,2,3,4) is associated with the n-th node, thenode having a first node and a fourth node as child nodes is defined asa 14th node, the node having a second node and a third node as childnodes is defined as a 23rd node, and the node having the 14th node andthe 23rd node as child nodes is defined as the 1423rd node. Thus, thefirst to the fourth nodes are nodes that are constructed of the first,the second, the third, and the fourth ground contact portions 10,respectively, the 14th node is a node constructed of the first groundcontact portion 10 and the fourth ground contact portion 10, the 23rdnode is a node constructed of the second ground contact portion 10 andthe third ground contact portion 10, and the 1423rd node is the nodeconstructed of all the ground contact portions 10.

According to general designations in a tree structure, a node having nochild node is referred to as a leaf node, and a node having no parentnode is referred to as a root node. Thus, the n-th node (n=1,2,3,4) is aleaf node, while the 1423rd node is a root node. In the presentdescription, to identify nodes, leaf nodes will be assigned the samenumbers (1,2,3, . . . ) as those of the ground contact portions (orlegs) associated therewith, while nodes other than the leaf nodes willbe assigned with numbers that are greater than those of the leaf nodes.Further, the nodes other than the leaf nodes and the root node will bereferred to as intermediate nodes. In the first embodiment, theintermediate nodes are the 14th node and the 23rd node.

Input to the desired floor reaction force distributor 102 are a desiredtotal floor reaction force central point trajectory, a desired totalfloor reaction force trajectory and a desired ground contact portiontrajectory out of a desired gait determined by the gait generatingdevice 100. The gait parameters (the estimated landing position, anestimated landing time, etc. of the ground contact portion 10 of a freeleg of the robot 1) used by the gait generating device 100 to determinethe desired gait may be also input to the desired floor reaction forcedistributor 102. Then, on the basis of these inputs, the desired floorreaction force distributor 102 determines desired node floor reactionforce central points (desired n-th node floor reaction force centralpoints) Qn (n=1,2,3,4,14,23,1423), which are the desired positions ofthe floor reaction force central points of the nodes hierarchized asdescribed above, and weights Wn (n=1,2,3,4,14,23) of the nodes exceptthe root node. Each weight Wn will be a nonnegative value of 1 or less.

The technique for determining desired node floor reaction force centralpoints Qn (n=1,2,3,4,14,23) and the weights Wn (n=1,2,3,4,14,23) will beexplained in detail below with reference mainly to FIG. 3( b), FIG. 5and FIG. 7. FIG. 3( b) shows a relationship between Qn and Wn in a stateillustrated in the figure, FIGS. 5( a) to (c) show setting examples ofWn, and FIG. 7 shows a relationship between weights and desired nodefloor reaction force central points.

The desired node floor reaction force central point of the 1423rd node,which is the root node, (the desired 1423rd node floor reaction forcecentral point) Q1423 is determined to be identical to the desired totalfloor reaction force central point (point P in FIG. 3 and FIG. 4)(P=Q1423). Accordingly, in the following explanation, the same referencecharacter P as that of a desired total floor reaction force centralpoint will be frequently used as the mark denoting the desired 1423rdnode floor reaction force central point.

Further, a weight W1423 of the root node is set to “1” for the sake ofconvenience in order to maintain the uniformity of expressions.

In the present embodiment, the desired floor reaction force centralpoint of an n-th ground contact portion 10 (n=1,2,3,4) (also referred toas a desired n-th ground contact portion floor reaction force centralpoint) is always set to the central point of an n-th ground contactportion 10 (the central point of the spherical joint 12). Further, thedesired node floor reaction force central point Qn (n=1,2,3,4) of a leafnode associated with the n-th ground contact portion 10 is determined tobe identical to the desired floor reaction force central point of then-th ground contact portion 10. Hence, the desired node floor reactionforce central point Qn (n=1,2,3,4) is also always set to the centralpoint of the n-th ground contact portion 10 (the position of this pointis uniquely determined from a desired ground contact portion position ofthe n-th ground contact portion 10). The position of the desired nodefloor reaction force central point Qn (n=1,2,3,4) of the leaf nodedetermined as described above is uniquely determined from the desiredground contact portion position of the n-th ground contact portion 10.In other words, the desired node floor reaction force central point Qn(n=1,2,3,4) of a leaf node defines the desired position of the n-thground contact portion 10. In the following explanation, the desirednode floor reaction force central point of a leaf node may be referredto as a desired ground contact portion floor reaction force centralpoint in some cases. Each desired ground contact portion trajectory isset such that it continuously changes, so that Qn (n=1,2,3,4) will alsocontinuously change.

Supplementally, in the present embodiment, the desired floor reactionforce central point Qn (n=1,2,3,4) of an n-th leaf node (an n-th groundcontact portion 10) has been set to the central point of the n-th groundcontact portion 10; however, in a robot that allows the postures ofground contact portions to be controlled, the desired floor reactionforce central point of each leaf node (each ground contact portion) maybe set, for example, within the ground contact surface of the groundcontact portion defined by the desired position/posture of the groundcontact portion associated with the leaf node (the surface that comes incontact with a floor surface assumed in a desired gait).

Hereinafter, in general, a segment that connects arbitrary two points Aand B or the length thereof will be denoted as AB. In addition, anoperator “*” means multiplication for a pair of scalar and scalar or apair of scalar and vector, while it means an outer product (vectorproduct) for a pair of vector and vector.

The weight Wn (n=1,2,3,4,14,23) of each node except the root node anddesired node floor reaction force central points Q14 and Q23 of theintermediate nodes are determined such that a desired 14th node floorreaction force central point Q14 will be the internally dividing pointof a segment Q1Q4 that satisfies a relational expression,Q1Q14:Q14Q4=W4:W1=(1−W1):W1, a desired 23rd node floor reaction forcecentral point Q23 will be the internally dividing point of a segmentQ2Q3 that satisfies a relational expression,Q2Q23:Q23Q3=W3:W2=(1−W2):W2, and a desired 1423rd node floor reactionforce central point P (=desired total floor reaction force centralpoint) will be the internally dividing point of a segment Q14Q23 thatsatisfies Q14P:PQ23=W23:W14=(1−W14):W14.

In other words, Q14, Q23 and Wn (n=1,2,3,4,14,23) are determined tosatisfy at least the following expressions 1, 2 and 3. Incidentally, Qn(n=1,2,3,4,14,23) in these expressions 1 to 3 means the position(positional vector) of the point.Q14=Q1*W1+Q4*W4, W1+W4=1  Expression 1Q23=Q2*W2+Q3*W3, W2+W3=1  Expression 2P=Q14*W14+Q23*W23, W14+W23=1  Expression 3

Wn (n=1,2,3,4,14,23) takes a nonnegative value of 1 or less, so that thecoefficients (weights) of Qn (n=1,2,3,4,14,23) in the right sides of theabove expressions 1, 2 and 3 will be all nonnegative values.

The above expressions 1 to 3 mean that the position of a desired nodefloor reaction force central point of each node having child nodes (thatis, each node that is not a leaf node) is set to a weighted averageposition of the position of the desired node floor reaction forcecentral points of the child nodes of the node by using a predeterminednonnegative weight. In other words, as shown in FIG. 3( b) and FIG. 7mentioned above, the desired node floor reaction force central point Qn(n=14,23,1423) of each node having child nodes is set at the internallydividing point of the desired node floor reaction force central pointsof all the child nodes of the node. FIG. 7 is a diagram showing arelationship between the desired node floor reaction force centralpoints Qn (n=1,2,3,4,14,23,1423) of nodes and the weight Wn(n=1,2,3,4,14,23). Incidentally, L23, L14 and L1423 in FIG. 3( b) denotesegments Q2Q3, Q1Q4 and Q23Q14, respectively.

Supplementally, Q1 to Q4 and P (=Q1423) are determined as describedabove, so that once Wn (n=1,2,3,4,14,23) is determined, Q14 and Q23 thatsatisfy expressions 1 to 3 are uniquely determined. In other words, ifQ14 and Q23 are determined, then Wn (n=1,2,3,4,14,23) satisfyingexpressions 1 to 3 will be uniquely determined. Accordingly, the weightWn (n=1,2,3,4,14,23) may be determined and then the desired node floorreaction force central points Q14 and Q23 of intermediate nodesaccording to the above expressions 1 to 3 may be determined, or thedesired node floor reaction force central points Q14 and Q23 of theintermediate node may be determined and then the weight Wn(n=1,2,3,4,14,23) may be determined according to the above expression 1to 3. Either way may be used.

The desired node floor reaction force central points Q14 and Q23 of theintermediate nodes move as the ground contact portions 10 move, as shownin time series in FIG. 3(a) to FIG. 3( c) and FIG. 4( a) to FIG. 4( c).Weights W14 (=1−W23), W1 (=1−W4) and W3 (=1−W2) at this time aredetermined such that they continuously change, as shown in, for example,the graphs of FIGS. 5( a) to (c), respectively. The trajectory of Qn andthe weight Wn of the n-th node (n=1,2,3,4) are set such that theycontinuously change, so that the desired node floor reaction forcecentral points Q14 and Q23 are also set such that they continuouslymove. This means that all desired node floor reaction force centralpoints (desired n-th node floor reaction force central points(n=1,2,3,4,14,23,1423)) are set such that they continuously move.

The desired floor reaction force distributor 102 outputs the desirednode floor reaction force central point of each node determined asdescribed above. The desired floor reaction force central point of theroot node is the same as the desired total floor reaction force centralpoint determined by the gait generating device 100, so that it does nothave to be output from the desired floor reaction force distributor 102.

The desired floor reaction force distributor 102 determines and outputsa desired node floor reaction force, which is the desired value of thefloor reaction force acting on the desired floor reaction force centralpoint of each node. The desired node floor reaction force that is outputincludes at least a desired node floor reaction force (a desired n-thnode floor reaction force) acting on a desired node floor reaction forcecentral point Qn of an n-th node (n=1,2,3,4), which is a leaf node, thatis, the desired floor reaction force of each ground contact portion 10.This desired node floor reaction force is necessary primarily tocompensate for the flexures of the compliance mechanisms 42 or the like(refer to FIG. 1) of the legs #1 to #4 (more specifically, compensationfor the positional displacement of the ground contact portions 10 causedby the flexures of the compliance mechanisms 42 and the link mechanismsof the legs), the compensation being carried out by the processing of ahierarchical compliance operation determiner 114, which will bediscussed later. Supplementally, if the robot 1 is provided withcompliance mechanisms in addition to those on the distal portions of thelegs #1 to #4, then preferably, a desired 14th node floor reaction forceand a desired 23rd node floor reaction force (the desired node floorreaction forces of intermediate nodes) are also determined and output inorder to determine the deformations of the compliance mechanisms.

Generally, a desired floor reaction force (a desired node floor reactionforce) acting on a desired node floor reaction force central point ofeach node may be determined from a desired total floor reaction forceand the weight of each node. Specifically, the desired floor reactionforce of any one node may be determined by multiplying the product ofthe weight of the node and the weight of all ancestor nodes of the nodeby a desired total floor reaction force. More specifically, a desiredn-th node floor reaction force is calculated according to the followingexpression 4a (or expression 4b).Desired n-th node floor reaction force =Weight of the n-th node*Productof the weight of all ancestor nodes of the n-th node*Desired total floorreaction force  Expression4aDesired n-th node floor reaction force =Weight of the n-th node*Productof the weight of all ancestor nodes (excluding the root node) of then-th node*Desired total floor reaction force  Expression4b

Alternatively, desired node floor reaction forces are determined suchthat the desired floor reaction force of an arbitrary n-th node that isnot a leaf node agrees with the sum (resultant force) of the desiredfloor reaction forces of all child nodes of the n-th node and thedesired floor reaction force of the root node agrees with a desiredtotal floor reaction force. This relationship is shown in FIG. 8.

More specifically, if a desired n-th node floor reaction force, which isthe desired floor reaction force of an n-th node, is denoted by Fn(n=1,2,3,4,14,23,1423) and a desired total floor reaction force isdenoted by Ftotalref, then Fn is determined from Ftotalref and theweight Wn (n=1,2,3,4,14,23) according to the expressions shown in FIG.8. The expressions in FIG. 8 are equivalent to the above expression 4aor 4b.

Determining the desired floor reaction force central point of each node(a desired node floor reaction force central point) and the desiredfloor reaction force of each node (a desired node floor reaction force)as described above is to determine the desired floor reaction forcecentral point and the desired floor reaction force of each node suchthat the horizontal component of the moment generated about a desiredn-th node floor reaction force central point by the resultant force ofthe desired floor reaction forces of all child nodes of an n-th nodebecomes zero. Accordingly, the moment horizontal component of a desirednode floor reaction force is zero for any nodes.

A set of each desired node floor reaction force central point Qn(n=1,2,3,4,14,23), each weight Wn(n=1,2,3,4,14,23) and each desired nodefloor reaction force Fn (n=1,2,3,4,14,23) is determined such that thedesired floor reaction force of the ground contact portion 10 not incontact with the ground (in a no-contact-with-ground state) (the desirednode floor reaction force of a leaf node associated with the groundcontact portion 10 not in contact with the ground) is zero. Hence, inthe first embodiment, the weight W14 is set to zero (W23=1) during aperiod in which the first ground contact portion 10 and the fourthground contact portion 10 are free legs and off a floor (the groundcontact portions 10 of the free legs are moving in the air), while theweight W23 is set to zero (W14=1) during a period in which the secondground contact portion 10 and the third ground contact portion 10 arefree legs and off a floor, as shown in FIG. 5( a). Thus, the weight ofan intermediate node having child nodes is set to zero during a periodin which none of the ground contact portions belonging as leaf nodes tothe intermediate node are in contact with the ground. In other words,the weight of an intermediate node is not set to zero during a period inwhich any one of the ground contact portions belonging to theintermediate node is in contact with the ground (strictly speaking, aperiod during which a non-zero floor reaction force is acting on any oneof the ground contact portions).

Supplementally, the first embodiment does not have a period during whichonly one of the first ground contact portion 10 and the fourth groundcontact portion 10 belonging to the intermediate node Q14 is placed in ano-contact-with-ground state; therefore, the weights W1 and W4corresponding to these ground contact portions 10 and 10 do not have anyperiods during which they are 0 or 1. If, however, a desired groundcontact portion trajectory is determined so as to include a periodduring which only one of the first ground contact portion 10 and thefourth ground contact portion 10 will be placed in theno-contact-with-ground state, then the weight associated with the groundcontact portion 10 that will be placed in the no-contact-with-groundstate during the period may be set to 0, and the weight associated withthe ground contact portion 10 to be in contact with the ground may beset to 1. In this case, the weight of the intermediate node Q14 duringthat period will be set to a non-zero value. The same applies to theweights associated with the intermediate node Q23 and the second groundcontact portion 10 and the third ground contact portion 10, which areleaf nodes belonging thereto. Generally speaking, the weight of a nodehaving child nodes is set to a non-zero value if any one of the groundcontact portions belonging to the node is in contact with the ground,while it is set to zero if all of the ground contact portions belongingto the node are in no-contact-with the ground.

The desired node floor reaction forces determined as described abovecontinuously change, making them suited for achieving a movement(walking) of the robot 1 with less impact.

As described above, according to the first embodiment, each desired nodefloor reaction force central point Qn (n=1,2,3,4,14,23,1423), eachweight Wn(n=1,2,3,4,14,23) and each desired each node floor reactionforce Fn (n=1,2,3,4,14,23,1423) are determined by the desired floorreaction force distributor 102 such that they satisfy the followingconditions A) to G).

-   A) The desired node floor reaction force central point Qn(n=1,2,3,4)    of each leaf node agrees with the central point of the ground    contact portion 10 corresponding to the leaf node. More generally,    Qn(n=1,2,3,4) is determined according to a desired gait (a desired    motion, such as a desired ground contact portion trajectory).    Supplementally, for example, the desired floor reaction force    central point of each ground contact portion 10 may be determined by    the gait generating device 100, and in this case, the desired node    floor reaction force central point Qn(n=1,2,3,4) may be determined    on the basis of the desired floor reaction force determined by the    gait generating device 100.-   B) The desired node floor reaction force central point of the root    node agrees with a desired total floor reaction force central point    P.-   C) The desired node floor reaction force central point    Qn(n=1,2,3,4,14,23) and the weight Wn(n=1,2,3,4,14,23) of each node    excluding the root node satisfy the relational expressions of the    above expressions 1 to 3. In other words, the desired node floor    reaction force central point Qn of an arbitrary n-th node    (n=14,23,1423) having child nodes will be a weighted average point    of the desired node floor reaction force central points of the child    nodes of the n-th node.-   D) The desired node floor reaction force Fn of an arbitrary n-th    node (n=14,23,1423) having child nodes agrees with the sum    (resultant force) of the desired node floor reaction forces of all    child nodes of the n-th node, and the desired node floor reaction    force F1423 of the root node (the 1423rd node) agrees with the    desired total floor reaction force Ftotalref. In other words, the    desired floor reaction force Fn and the weight Wn of the n-th node    (n=1,2,3,4,14,23,1423) satisfy the relational expressions in FIG. 8.-   E) The desired node floor reaction force of a leaf node associated    with the ground contact portion 10 not in contact with the ground is    zero.-   F) Qn, Wn, and Fn (n=1,2,3,4,14,23,1423) continuously change.-   G) The weight of a leaf node corresponding to the ground contact    portion 10 in a no-contact-with-ground state or the weight of any    one of the ancestor nodes of the leaf node is zero.

Incidentally, a desired node floor reaction force may be determined onthe basis of each desired node floor reaction force central point inplace of determining it on the basis of a weight as described above.More specifically, each desired node floor reaction force central pointmay be determined such that the aforesaid conditions A) to C) aresatisfied, then each weight may be determined on the basis of thedesired node floor reaction force central point and the aforesaidexpressions 1 to 3, and then the desired node floor reaction force maybe determined according to the aforesaid expression 4 by using thedetermined weight.

Returning to the explanation of FIG. 2, the posture error calculator 103calculates the error of an actual body posture with respect to a desiredbody posture and outputs the calculated error to the robot 1. In thepresent embodiment, the posture error calculator 103 receives aninclination angle of the body 24 relative to the vertical directiondetected by the posture sensor 36 (hereinafter referred to as the actualbody posture inclination) and a desired body position/posture determinedby the gait generating device 100 (specifically, the inclination anglein a desired body posture relative to the vertical direction;hereinafter referred to as the desired body posture inclination), andthen calculates an error θberr between them (Actual body postureinclination —Desired body posture inclination; hereinafter referred toas the body posture inclination error θberr). The calculated θberr iscomposed of a component about the X axis (the component in the rolldirection) θberrx and a component about the Y axis (the component in thepitch direction) θberry. If a desired body posture inclination is, forexample, steadily zero, then θberr=actual body posture inclination, sothat a detected value (actual body posture inclination) of a posturesensor 36 may be directly output as the body posture inclination errorθberr.

Generally speaking, the posture stabilization control calculator 104calculates a compensating total floor reaction force, which is acompensation amount of a total floor reaction force (the correctionamount of a desired total floor reaction force) for stabilizing theposture of the robot 1 according to the state of the robot 1 that isdetected or estimated on the basis of the information of a sensorprovided in the robot 1, such as the aforesaid body posture inclinationerror.

To stabilize the posture of the robot 1 in the long term, atranslational force and a moment necessary to restore the actualposition/posture of a predetermined portion, such as the body 24, of therobot 1 to a desired position/posture are determined, and these have tobe additionally generated at the desired total floor reaction forcecentral point (desired ZMP) being the point of action. The additionaltranslational force and moment are referred to as a compensating totalfloor reaction force. The moment component of a compensating total floorreaction force is referred to as a “compensating total floor reactionforce moment Mdmd (specifically, a compensating total floor reactionforce moment Mdmd about a desired total floor reaction force centralpoint (desired ZMP)).

In the present embodiment, the posture stabilization control calculator104 calculates the compensating total floor reaction force moment Mdmdso as to restore (approximate) an actual body posture inclination to adesired body posture inclination. Hence, the body posture inclinationerrors θberr (θberrx, θberry) determined by the posture error calculator103 are input to the posture stabilization control calculator 104.

The posture stabilization control calculator 104 calculates thecompensating total floor reaction force moment Mdmd on the basis of theinput body posture inclination error θberr. The calculated Mdmd iscomposed of a component about the X axis Mdmdx and a component about theY axis Mdmdy.

Specifically, Mdmdx and Mdmdy are determined by, for example, thefeedback control law (PD control law here) of expressions 5 and 6 givenbelow. More specifically, Mdmdx and Mdmdy are determined so as toapproximate the body posture inclination errors θberrx and θberry tozero.Mdmdx=−Kthx*θberrx−Kwx*(dθberrx/dt)  Expression 5Mdmdy=−Kthy*θberry−Kwy*(dθberry/dt)  Expression 6where Kthx, Kthy, Kwx and Kwy denote predetermined gains. Further,(dθberrx/dt) and (dθberry/dt) denote the time differential values of thebody posture inclination errors θberrx and θberry, respectively.

In the present embodiment, a component of the compensating total floorreaction force moment Mdmd about the Z axis (the component in the yawdirection) Mdmdz is not used, so that Mdmdz is not determined; however,Mdmdz may be determined to prevent the robot 1 from spinning (slippageabout the vertical axis). A method for determining Mdmdz is explained indetail in Japanese Patent Application No. 2003-185613 or Japanese PatentApplication No. 2003-185930 previously proposed by the presentapplicant. Moreover, for example, to restore the position of an actualcenter-of-gravity of the robot 1 to the position of thecenter-of-gravity in a desired gait, the translational force of acompensating total floor reaction force can be determined on the basisof a positional error of the center-of-gravity.

The floor reaction force detector 108 detects the actual floor reactionforces, which are the actual values of the floor reaction forces actingon the ground contact portions 10 of the actual robot 1 (that is, theactual floor reaction forces of leaf nodes (actual node floor reactionforces)) on the basis of the outputs of the six-axis force sensor 34 ofthe legs #1 to #4. In addition, the floor reaction force detector 108calculates the relative positions/postures (the relative positions inthe first embodiment) of the ground contact portions 10 relative to thecoordinate system fixed to the body 24 on the basis of the actual jointdisplacements of the joints 14 and 15 (the actual rotational angles ofthe joints 14 and 15 about individual rotational axes) of the legs #1 to#4 detected by sensors, such as encoders, (not shown) provided on thejoints 14 and 15 of the robot 1. At this time, joint displacementcommands, which are the displacement command values (rotational anglecommand values) of the joints 14 and 15 may be used in place of actualjoint displacements, or both actual joint displacements and jointdisplacement commands may be used. Then, based on the calculatedrelative positions/postures of the ground contact portions 10, thedetected values of the six-axis force sensor 34 of the legs #1 to #4(these being the values on a local coordinate system fixed to thesix-axis force sensor 34 or the like) are coordinate-converted tocalculate actual floor reaction forces represented on the coordinatesystem fixed to the body 24, and then the calculated actual floorreaction forces are converted into the actual floor reaction forcesrepresented on a supporting leg coordinate system (global coordinatesystem). For the coordinate conversion into the supporting legcoordinate system, detected values of the posture sensor 36 or desiredbody posture inclinations may be used. Supplementally, in the robot 1according to the first embodiment, no floor reaction force moment actson the central points of the ground contact portions 10, as describedabove, so that there is no need to detect a moment component in theactual floor reaction force of each ground contact portion 10. In thiscase, as mentioned above, in place of the six-axis force sensor 34, athree-axis force sensor may be used to detect translational forcecomponents of actual floor reaction forces in three axes, or a one-axisfloor reaction force sensor may be used to detect only translationalforce vertical components of actual floor reaction forces.

Based on a final desired trajectory of each ground contact portionposition/posture (this being determined by a hierarchical complianceoperation determiner 114, which will be discussed later) and desiredbody position/posture or the like, the robot geometric model (inversekinematics calculator) 110 performs inverse kinematics calculation tocalculate joint displacement commands, which are the command values ofthe displacements (rotational angles) of the joints 14 and 15 of therobot 1, that satisfy the above. In the present embodiment, the equationof the solution of the inverse kinematics calculation has beendetermined beforehand, and the joint displacement commands have beencalculated simply by substituting desired body position/posture and thefinal desired position of each ground contact portion into the equation.More specifically, the robot geometric model 110 receives a desired bodyposition/posture trajectory determined by the gait generating device 100and a corrected desired ground contact portion trajectory corrected aswill be discussed later by the hierarchical compliance operationdeterminer 114 (a corrected desired ground contact portion trajectorywith deformation compensation), and calculates the joint displacementcommands of the joints 14 and 15 of the legs #1 to #4 by the inversekinematics calculation from the received values.

In the case of a robot having joints, such as arm joints and a neckjoint, in addition to leg joints, as in the sixth embodiment to bediscussed later, the displacements of joints other than the leg jointsare determined by the inverse kinematics calculation on the basis of therelative positions/postures of hands, the head or the like with respectto the body.

The displacement controller 112 receives the actual joint displacements(detected values) of the joints 14 and 15 of the robot 1 and the jointdisplacement commands calculated by the robot geometric model (inversekinematics calculator) 110, and controls (feedback-control) actuators(not shown) of the joints 14 and 15 by using the joint displacementcommands as the desired values such that the actual joint displacementsfollow the desired values.

The hierarchical compliance operation determiner 114 corrects a desiredground contact portion trajectory to approximate an actual total floorreaction force to the resultant force of a desired total floor reactionforce and a compensating total floor reaction force, and outputs acorrected desired ground contact portion position/posture trajectorywith deformation compensation, which is the desired ground contactportion trajectory after the correction. In the present embodiment, thepostures of the ground contact portions 10 cannot be controlled;therefore, the corrected desired ground contact portion position/posturetrajectory with deformation compensation is actually a corrected desiredground contact portion position trajectory with deformationcompensation.

The hierarchical compliance operation determiner 114 generally correctsthe desired ground contact portion trajectories of the ground contactportions 10 so as to satisfy the following three requirements as much aspossible.

-   Requirement 1) In order to stabilize the position/posture of the    robot 1, an actual total floor reaction force is made to follow the    resultant force of a compensating total floor reaction force (moment    Mdmd) output by the posture stabilization control calculator 104 and    a desired total floor reaction force. According to the first    embodiment, in order to stabilize the posture inclination (the    inclination relative to the vertical direction) of the body 24 of    the robot 1, the horizontal component of an actual total floor    reaction force moment about a desired total floor reaction force    central point is made to follow horizontal components Mdmdx and    Mdmdy of the compensating total floor reaction force moment Mdmd.    Supplementally, the horizontal component of the desired total floor    reaction force moment about the desired total floor reaction force    central point is zero, so that the resultant force of this and Mdmd    agrees with Mdmd.-   Requirement 2) The absolute value of the actual floor reaction force    moment about a desired floor reaction force central point of each    node, which is not a leaf node, is minimized as much as possible to    prevent an actual floor reaction force from being focused on some    ground contact portions 10 of a plurality of ground contact portions    10 to be in contact with the ground, causing the actual floor    reaction force on other ground contact portions 10 to be excessively    reduced, which will lead to extremely deteriorated ground contacting    properties of the ground contact portions 10 having the reduced    actual floor reaction force. In the first embodiment, the absolute    values of the actual floor reaction force moments about the desired    floor reaction force central points of the 14th node, the 23rd node    and the 1423rd node are minimized as much as possible.-   Requirement 3) In order to secure the ground contacting properties    of the ground contact portions 10 to be in contact with the ground,    that is, to prevent local ground contacting properties of the ground    contact portions 10 from deteriorating due to uneven local ground    contact pressure distribution (the distribution of an actual floor    reaction force) in the ground contact portions 10, the absolute    values of the actual floor reaction force moments about the desired    floor reaction force central points of the ground contact portions    10 (leaf nodes) are minimized as much as possible. However, in the    robot 1 of the first embodiment, the actual floor reaction force    moments about the desired floor reaction force central points of the    ground contact portions 10 are always zero; therefore, it is    unnecessary to consider this requirement 3).

In general, it is impossible to fully satisfy all of requirements 1) to3) or requirements 1) and 2). For example, it is often physicallyimpossible to zero the actual floor reaction force moments about thedesired floor reaction force central points of the ground contactportions 10 while making an actual total floor reaction force agree withthe resultant force of a compensating total floor reaction force and adesired total floor reaction force. Hence, the hierarchical complianceoperation determiner 114 usually corrects the desired ground contactportion trajectories of the ground contact portions 10 at certaincompromise points while satisfying requirements 1) to 3) orrequirements 1) and 2) as much as possible.

The above has given the overviews of the functional means (functionalcomponents) of the control device 50.

Supplementally, the hierarchical compliance operation determiner 114,the posture stabilization control calculator 104 and a robot geometricmodel (inverse kinematics calculator) 110 correspond to the nodeoperation control means in the present invention.

Referring now to the flowchart of FIG. 9, the overall operation(arithmetic processing) of the control device 50 will be explained inmore detail. FIG. 9 is a flowchart (structured flowchart) showing themain routine processing of the control device 50. The components of thecontrol device 50 carrying out pertinent processing are shown on theleft in FIG. 9.

First, the initialization of the control device 50 is performed in S10,then the processing advances to S14 via S12. The arithmetic processingof the control device 50 waits for a timer interrupt for each controlcycle. The control cycle is, for example, 50 ms.

Subsequently, the processing proceeds to S16 wherein it determineswhether a gait change is observed, and if the determination result isNO, then it proceeds to S22, which will be discussed later. If thedetermination result in S16 is YES, then the processing proceeds to S18wherein it initializes time t to zero, and proceeds to S20 wherein itsets gait parameters. In the present embodiment, for example, a desiredgait of a predetermined period from the moment a predetermined leg(e.g., #1) of the robot 1 leaves a floor to the moment it leaves thefloor next (or from the moment it lands to the moment it lands next) istaken as a unit, and gait parameters that are the parameters definingthe desired gait for the predetermined period (the parameters used inthe algorithm for determining the desired gait) are set in S20. The“gait change” mentioned in S16 means the change of the desired gait forthe predetermined period. Whether the desired gait is changing may bedetermined mainly on the basis of time or a detected value of thesix-axis force sensor 34 on the predetermined leg.

The gait parameters set in S20 are composed of motion parameters thatdefine the desired motion trajectories (specifically, a desired groundcontact portion trajectory and a desired body position/posturetrajectory) of the robot 1 and the floor reaction force parameters thatdefine desired floor reaction force trajectories (specifically, adesired total floor reaction force trajectory and a desired total floorreaction force central point trajectory). Supplementally, once thedesired motion of the robot 1 is determined, the desired total floorreaction force can be obtained by reversing the sign of the resultantforce of the inertial force generated by the desired motion and thegravity acting on the robot 1, so that the floor reaction forceparameters may be the ones that define only a desired total floorreaction force central point trajectory. Further, motion parameters donot have to include parameters that define a desired bodyposition/posture trajectory if a desired ground contact portiontrajectory (more generally, the parameter of a desired motion other thandesired body position/posture) and a desired total floor reaction forcecentral point are determined, and then desired body position/posture aredetermined using a dynamic model of the robot 1 such that the horizontalcomponent of a moment generated about a desired total floor reactionforce central point (desired ZMP) by the resultant force of the inertialforce generated by a desired motion, including desired bodyposition/posture of the robot 1, and the gravity acting on the robot 1becomes zero.

Subsequently, the processing proceeds to S22 wherein the instantaneousvalue of a desired gait is determined on the basis of the aforesaid gaitparameters. Here, “instantaneous value” means a value for each controlcycle, and a desired gait instantaneous value is composed of theinstantaneous values of desired body position/posture, a desired groundcontact portion position (the instantaneous value of a desired groundcontact portion trajectory), a desired total floor reaction force, and adesired total floor reaction force central point position (a desired ZMPposition). In the present embodiment, the postures of the ground contactportions 10 cannot be controlled, so that the instantaneous values ofthe desired postures of the ground contact portions 10 are notdetermined. In a case where the postures of the ground contact portionscan be controlled, the parameters defining the desired postures of theground contact portions may be included in the gait parameters and theinstantaneous values of the desired postures of the ground contactportions may be determined on the basis of the parameters.

The processing of S14 to S22 described above is the processing carriedout by the gait generating device 100.

Subsequently, the processing proceeds to S24 wherein the weightWn(n=1,2,3,4,14,23) of each node and a desired floor reaction forcecentral point of each node (a desired node floor reaction force centralpoint) Qn(n=1,2,3,4,14,23) are determined. This processing is theprocessing carried out as described above by the desired floor reactionforce distributor 102.

Subsequently, the processing proceeds to S26 wherein desired node floorreaction forces (including at least the desired floor reaction forces ofthe ground contact portions 10 (leaf nodes)) are determined. In thefirst embodiment, the desired node floor reaction forces of leaf nodes(desired ground contact portion floor reaction forces) are determined.This processing of S26 is also the processing carried out as describedabove by the desired floor reaction force distributor 102. As describedabove, if the compliance mechanisms are provided on other portions inaddition to the distal portions of the legs #1 to #4, then the desiredfloor reaction forces of intermediate nodes that are not leaf nodesshould be also determined. Supplementally, the moment horizontalcomponent of a desired node floor reaction force is zero.

Subsequently, the processing proceeds to S28 wherein the state of therobot 1, such as the actual body posture inclination, is detected froman output of the posture sensor 36 or the like. In the first embodiment,the value of an actual body posture inclination detected by the posturesensor 36 is captured by the posture error calculator 103, and a bodyposture inclination error θberr is calculated from the detected valueand the desired body posture inclination (the instantaneous value at thecurrent time) out of the desired body position/posture.

Subsequently, the processing proceeds to S30 wherein a compensatingtotal floor reaction force for stabilizing the posture of the robot 1 isdetermined from the state of the robot 1 detected in S28. In the firstembodiment, the horizontal components Mdmdx and Mdmdy of a compensatingtotal floor reaction force moment Mdmd about a desired total floorreaction force central point (desired ZMP) are calculated by the posturestabilization control calculator 104 from the body posture inclinationerror θberr according to the above expression 5 and expression 6.

Subsequently, the processing proceeds to S32 wherein the actual floorreaction force of each ground contact portion 10 is detected. This isthe processing carried out by the actual floor reaction force detector108. As described above, the actual floor reaction force for each groundcontact portion 10 detected by the six-axis force sensor 34 that isconverted to a supporting leg coordinate system (global coordinatesystem) is determined. Hereinafter, the actual floor reaction force ofeach ground contact portion 10 may be referred to as an actual groundcontact portion floor reaction force in some cases.

Subsequently, from S34 to S38, the processing of the hierarchicalcompliance operation determiner 114 is carried out.

For a while, the following will explain the overall processing of thehierarchical compliance operation determiner 114 before the processingof these S34 to S38 is specifically explained. In this explanation, forthe convenience of understanding, the state shown in FIG. 3( b)mentioned above (the state wherein all the legs #1 to #4 of the robot 1are supporting legs) will be primarily taken as an example.

The hierarchical compliance operation determiner 114 determines thetranslational force components and moment components of the desired nodefloor reaction forces of nodes excluding leaf nodes (more specifically,the translational force components and the moment components that havedesired node floor reaction force central points as the points ofaction) mainly on the basis of the desired node floor reaction forces ofthe leaf nodes determined by the desired floor reaction forcedistributor 102.

The translational force components of the desired floor reaction forces(desired node floor reaction forces) of the nodes in the state shown inFIG. 3( b) are illustratively shown in FIG. 10. In this figure, a vectorFn_ref (n=1,2,3,4,14,23) denotes the translational force component of adesired n-th node floor reaction force. Further, Ftotalref denotes thetranslational force component of a desired total floor reaction force(=desired 1423rd node floor reaction force). As shown in FIG. 8mentioned above, a desired n-th node floor reaction force of anarbitrary n-th node (n=14,23,1423), which is not a leaf node, isdetermined by the resultant force of the desired node floor reactionforces of all child nodes of the n-th node. Therefore,F14ref=F1ref+F4ref, F23ref=F2ref+F3ref, andFtotalref(=F1423ref)=F14ref+F23ref. The hierarchical complianceoperation determiner 114 determines the translational force componentsF14ref(=F1ref+F4ref) and F23ref (=F2ref+F3ref) of the desired node floorreaction forces of intermediate nodes, as described above, from thetranslational force components Fn_ref(n=1,2,3,4) of the desired nodefloor reaction forces of the leaf nodes (the ground contact portion 10)determined by the desired floor reaction force distributor 102.Ftotalref (=F1423ref) is set to the translational force component of thedesired total floor reaction force determined by the gait generatingdevice 100.

Furthermore, the hierarchical compliance operation determiner 114determines the moment component of a desired node floor reaction forceof each node, excluding leaf nodes, having a desired node floor reactionforce central point being the point of action, as in the case of thetranslational force component of a desired node floor reaction force. Inthis case, however, according to the definition of a desired node floorreaction force central point Qn (n=1,2,3,4,14,23), the moment horizontalcomponent of a desired n-th node floor reaction force is always set tozero. The moment horizontal component of a desired 1423rd node floorreaction force (=desired total floor reaction force) is also set tozero.

In the robot 1 of the present embodiment, the ground contact portions 10are engaged with the spherical joints 12 (free joints) at the distal endportions of the legs #1 to #4, so that floor reaction force moments(horizontal components and vertical components) cannot be generated inthe ground contact portions 10 (leaf nodes). For this reason, in thehierarchical compliance operation determiner 114, the moment verticalcomponents of the desired node floor reaction forces of the groundcontact portions 10 (leaf nodes) are also set to zero.

If a desired node floor reaction force moment vertical component of anode that is not a leaf node is determined so as to dynamically balancewith a desired motion of the robot 1, then it could generally take anon-zero value; however, in the present embodiment, the control relatedto the rotation (the rotation in the yaw direction) of the posture ofthe robot 1 about the vertical axis is not carried out. In the presentembodiment, therefore, the setting of the vertical component of themoment of a desired node floor reaction force of a node that is not aleaf node will be omitted. For this reason, the moment components ofdesired node floor reaction forces in the state shown in FIG. 3( b) willnot be shown. If the control related to the rotation of the posture ofthe robot 1 about the vertical axis is carried out, then the desiredfloor reaction force moment vertical components of the nodes should bealso set.

Supplementally, if the translational force components and the momentcomponents of the desired node floor reaction forces of the nodes,including leaf nodes are determined by the desired floor reaction forcedistributor 102, then the determination need not be performed by thehierarchical compliance operation determiner 114.

Further, the hierarchical compliance operation determiner 114 alsodetermines the translational force component and the moment component ofan actual node floor reaction force, which is the actual floor reactionforce of each node.

The translational force components of the actual floor reaction forcesof the nodes (actual node floor reaction forces) in the state shown inFIG. 3( b) are illustratively shown in FIG. 11. In the diagram, a vectorFn_act (n=1,2,3,4,14,23) denotes a translational force component of anactual n-th node floor reaction force. Further, Ftotalact denotes thetranslational force component of an actual total floor reaction force(=actual 1423rd node floor reaction force). In general, thetranslational force component of the actual floor reaction force of eachnode that is not a leaf node is the resultant force of the translationalforce components of the actual floor reaction forces of all child nodesof the node. Accordingly, the translational force components of theactual floor reaction forces of the 14th node, the 23rd node, and the1423rd node are F14act=F1act+F4act, F23act=F2act+F3act,Ftotalact(=F1423act) =F14act+F23act. The translational force componentsF1act, F2act, F3act and F4act of the actual floor reaction forces of theleaf nodes are the translational force components of the actual floorreaction forces (actual ground contact portion floor reaction forces) ofthe ground contact portions 10 obtained by the actual floor reactionforce detector 108. The vectors shown by dashed lines in FIG. 11indicate the translational force components of the desired node floorreaction forces shown in FIG. 10 mentioned above. The hierarchicalcompliance operation determiner 114 determines the translational forcecomponents of the actual node floor reaction forces of the nodes fromthe actual floor reaction forces of the ground contact portions 10obtained by the actual floor reaction force detector 108.

The moment components of the actual floor reaction forces of the nodesin the state shown in FIG. 3( b) are illustratively shown in FIG. 12. Inthe diagram, a vector Mn_act (n=14,23,1423) denotes the moment componentof an actual n-th node floor reaction force. In this case, as with thetranslational force components of the actual floor reaction forces ofthe nodes, in general, the moment component of an actual floor reactionforce M14act, M23act or M1423act (=Mtotalact) of a node that is not aleaf node is defined as the moment component of the resultant force ofthe actual floor reaction forces of all child nodes of the node (themoment component having the desired floor reaction force central pointQ14, Q23 or Q1423 of the node as the point of action).

In the robot 1 of the present embodiment, no floor reaction force momentcan be generated in the ground contact portions 10 (leaf nodes), asdescribed above, so that the moment components of the actual floorreaction forces of leaf nodes (actual ground contact portion floorreaction forces) will be always zero. Thus, the moment components of theactual node floor reaction forces of the leaf nodes will not be shown.

The actual floor reaction force moment components of nodes that are notleaf nodes (in the present embodiment, M14act, M23act and M1423act) arenot generally zero. For example, a moment is usually generated about thedesired 14th node floor reaction force central point Q14 by thehorizontal components of the translational force components of theactual floor reaction forces of the first ground contact portion 10 (the1st node) and the fourth ground contact portion 10 (the 4th node).However, in the robot 1 in the present embodiment, the distal endportions of the legs are provided with the spherical joints 12, whichare free joints, so that a component in the same direction as a segmentQ1Q4 of M14act and a component in the same direction as a segment Q2Q3of M23act will be zero.

In a robot having the distal joints of its legs provided with actuators(a robot in which the postures of the distal ground contact portions ofthe legs are controllable), even if the control related to the rotationof the posture of the robot about the vertical axis is not carried out,the desired floor reaction force moment horizontal components of theground contact portions are also set and the actual floor reaction forcemoments of the ground contact portions are also detected, as shown in anembodiment disclosed in Japanese Unexamined Patent ApplicationPublication No. H10-277969 previously proposed by the present applicant.Then, an operation for correcting the postures of the ground contactportions should be performed so as to approximate the actual floorreaction force moment horizontal component of each ground contactportion to a desired floor reaction force moment horizontal component orto approximate the actual floor reaction force moment of each groundcontact portion to the sum (the vector sum) of the desired floorreaction force moment horizontal component and its ground contactportion compensating floor reaction force moment.

The processing of the hierarchical compliance operation determiner 114will be explained in more detail. In this case, a situation is assumedin which the posture of the body 24 of the robot 1 is about to falltoward left rear in the state shown in FIG. 3( b) and the compensatingtotal floor reaction force moment Mdmd determined by the posturestabilization control calculator 104 is as shown in FIG. 13.

To restore the posture (the inclination relative to the verticaldirection) of the body 24 of the robot 1 (to restore the inclination ina desired body posture), the horizontal component of an actual totalfloor reaction force moment about a desired total floor reaction forcecentral point (a desired ZMP) may be made to follow the horizontalcomponent of the sum of a desired total floor reaction force momentMtotalref (=M1423ref) and a compensating total floor reaction forcemoment Mdmd.

Meanwhile, at the desired total floor reaction force central point (thedesired ZMP), the horizontal component of the desired total floorreaction force moment Mtotalref is zero. Hence, to restore the posture(inclination) of the body 24 of the robot 1 in the longitudinal andlateral directions, the horizontal component of the actual total floorreaction force moment about the desired total floor reaction forcecentral point (the desired ZMP) may be made to follow the horizontalcomponents (Mdmdx, Mdmdy) of Mdmd. Further, in the present embodiment,the actual floor reaction force moment about the desired floor reactionforce central point of each ground contact portion 10 is zero.

Therefore, the hierarchical compliance operation determiner 114 in thefirst embodiment corrects the desired ground contact portion position(especially the position in the height direction) of each ground contactportion 10 determined by the gait generating device 100 so as to satisfythe aforesaid requirements 1) and 2) as much as possible.

To make the correction, the hierarchical compliance operation determiner114 determines a compensating angle. The compensating angle is amanipulated variable (a rotational amount) for correcting the relativerelationship among the positions of the ground contact portions 10 bythe operation of rotation about a certain point (correcting in thevertical direction in the present embodiment). In the presentembodiment, there are a 14th node compensating angle θ14, a 23rd nodecompensating angle θ23, and a 1423rd node compensating angle θ1423 asthe compensating angles. In other words, they are the compensatingangles of the nodes other than leaf nodes. Then, the hierarchicalcompliance operation determiner 114 determines a desired n-th node floorreaction force central point Qn′ obtained by correcting the desired n-thnode floor reaction force central point Qn (n=1,2,3,4,14,23) on thebasis of these compensating angles θ14, θ23 and θ1423.

As shown in FIG. 15, the 14th node compensating angle θ14 is the angleformed by a segment Q1Q4 and a segment Q1′ Q4′, and the 23rd nodecompensating angle θ23 is the angle formed by a segment Q2Q3 and asegment Q2′Q3′. Further, as shown in FIG. 16, the 1423rd nodecompensating angle θ1423 is the angle formed by a segment Q14Q23 and asegment Q14′Q23′. The technique for determining these node compensatingangles θ14, θ23 and θ1423 will be described later.

Referring to FIG. 15 and FIG. 16, the correction of the desired n-thnode floor reaction force central point Qn (n=1,2,3,4,14,23) on thebasis of the compensating angles θ14, θ23 and θ1423 is made as follows.

Referring to FIG. 15, a normal vector V14 of a plane that includes adesired first node floor reaction force central point Q1 (the desiredfloor reaction force central point of a first ground contact portion 10)and a desired fourth node floor reaction force central point Q4 (thedesired floor reaction force central point of a second ground contactportion 10) and that is perpendicular to a horizontal plane isdetermined. The magnitude of V14 is 1. The coordinate (position) of thedesired first node floor reaction force central point Q1 is rotationallymoved about a normal vector V14 with the desired 14th node floorreaction force central point Q14 as the center of rotation (about theaxis that passes Q14 and is parallel to V14) by the aforesaid 14th nodecompensating angle θ14. The point after Q1 is moved by the aboverotational movement is defined as Q1′ . Similarly, the coordinate(position) of the desired fourth node floor reaction force central pointQ4 is rotationally moved about the normal vector V14 with the desired14th node floor reaction force central point as the center of rotationby the 14th node compensating angle θ14. The point after Q4 is moved bythe above rotational movement is defined as Q4′. In other words, theends of a segment obtained by rotating the segment Q1Q4 about V14 by θ14with Q14, which is an internally dividing point thereof, as the centerof rotation are defined as Q1′ and Q4′. Thus, the 14th node compensatingangle θ14 is a manipulated variable for moving the relative relationshipof the positions of the desired floor reaction force central points Q1and Q4 of the first node and the fourth node, respectively, which arechild nodes of the 14th node, without moving the position of the desiredfloor reaction force central point Q14 of the 14th node.

Moreover, a normal vector V23 of a plane that includes a desired secondnode floor reaction force central point Q2 and a desired third nodefloor reaction force central point Q3 and that is perpendicular to ahorizontal plane is determined. The magnitude of V23 is 1. Thecoordinate (position) of the desired second node floor reaction forcecentral point Q2 is rotationally moved about the normal vector V23 withthe desired 23rd node floor reaction force central point Q23 as thecenter of rotation (about the axis that passes Q23 and is parallel toV23) by the 23rd node compensating angle θ23. As shown in FIG. 15, thepoint after Q2 is moved by the above rotational movement is defined asQ2′. Similarly, the coordinate (position) of the desired third groundcontact portion floor reaction force central point Q3 is rotationallymoved about the normal vector V23 with the desired 23rd node floorreaction force central point as the center of rotation by the 23rd nodecompensating angle θ23. The point after Q3 is moved by the aboverotational movement is defined as Q3′. In other words, the ends of asegment obtained by rotating the segment Q2Q3 about V23 by θ23 with Q23,which is an internally dividing point thereof, as the center of rotationare defined as Q2′ and Q3′. Thus, the 23rd node compensating angle θ23is a manipulated variable for moving the relative relationship of thepositions of the desired floor reaction force central points Q2 and Q3of the second node and the third node, respectively, which are childnodes of the 23rd node, without moving the position of the desired floorreaction force central point Q23 of the 23rd node.

Next, referring to FIG. 16, a normal vector V1423 of a plane thatincludes a desired 14th node floor reaction force central point Q14 anda desired 23rd node floor reaction force central point Q23 and that isperpendicular to a horizontal plane is determined. The magnitude ofV1423 is 1. The coordinate (position) of the desired 14th node floorreaction force central point Q14 is rotationally moved about the normalvector V1423 with the desired total floor reaction force central point P(=Q1423) as the center of rotation (about the axis that passes P and isparallel to V1423) by the 1423rd compensating angle θ1423. The pointafter Q14 is moved by the above rotational movement is defined as Q14′.Similarly, the coordinate (position) of the desired 23rd ground contactportion floor reaction force central point Q23 is rotationally movedabout the normal vector V1423 with the desired total floor reactionforce central point P as the center of rotation by the 1423rdcompensating angle θ1423. The point after Q23 is moved by the aboverotational movement is defined as Q23′. In other words, the ends of asegment obtained by rotating a segment Q14Q23 about V1423 by θ1423 withP(=Q1423), which is an internally dividing point thereof, as the centerof rotation are defined as Q14′ and Q23′. Thus, the 1423rd nodecompensating angle θ1423 is a manipulated variable for moving therelative relationship of the positions of the desired floor reactionforce central points Q14 and Q23 of the 14th node and the 23rd node,respectively, which are child nodes of the 1423rd node, without movingthe position of the desired node floor reaction force central point P ofthe 1423rd node.

Hereinafter, generally, a vector whose start point is A and end point isB will be denoted as a vector A_B.

Next, a point Q1″ that would be obtained when Q1′ is moved for a vectorQ14_Q14′ is determined. Similarly, a point Q4″ that would be obtainedwhen Q4′ is moved for a vector Q14_Q14′ is determined. Further, a pointQ2″ that would be obtained when Q2′ is moved for a vector Q23_Q23′ isdetermined. Similarly, a point Q3″ that would be obtained when Q3′ ismoved for a vector Q23_Q23′ is determined.

Subsequently, the desired ground contact portion position of an n-thground contact portion (n=1,2,3,4) is moved in parallel by a vectorQn_Qn″ (substantially vertical movement). This corrects the desiredground contact portion position of each ground contact portion 10.Supplementally, as with the desired floor reaction force central pointof each node, if the position of each leaf node is set to the positionof the ground contact portion 10 corresponding to the leaf node, and theposition of each node having child nodes is defined as a weightedaverage position of the positions of all child nodes of the node, usingthe weights set as described above, then the processing for moving thedesired floor reaction force central points of the child nodes of the14th node, the 23rd node and the 1423rd node, respectively, according tothe compensating angles θ14, θ23 and θ1423, as described above, isequivalent to moving the positions of the child nodes of the 14th node,the 23rd node and the 1423rd node, respectively, according to thecompensating angles θ14, θ23 and θ1423.

In a robot in which the distal end portions of its legs have no freejoints (a robot in which the postures of ground contact portions can becontrolled), the aforesaid parallel movement is performed withoutchanging the postures (desired postures) of ground contact portions, andthen the n-th ground contact portion is further rotated by a certainrotational angle θn_x about a longitudinal axis (X axis) and alsorotated about a lateral axis (Y axis) by a certain rotational angleθn_y, with Qn″ being the center. This corrects the desired postures ofground contact portions. The rotational angle θn (a two-dimensionalamount composed of θn_x and θn_y) in this case is referred to as an n-thground contact portion compensating angle, θn_x is referred to as ann-th ground contact portion compensating angle X component and θn_y isreferred to as an n-th ground contact portion compensating angle Ycomponent. The n-th ground contact portion compensating angle θn may bedetermined according to the technique described in Japanese UnexaminedPatent Application Publication No. H10-277969 previously proposed by thepresent applicant.

Supplementally, each of the node compensating angles is determined suchthat a resultant force of a desired floor reaction force moment (thehorizontal component thereof is zero) and a node compensating floorreaction force moment Mn_dmd is generated at the desired node floorreaction force central point of the node, which is the point of action.In this case, the compensating total floor reaction force moment Mdmd,which provides the basis of the node compensating floor reaction forcemoment Mn_dmd, is determined so as to approximate an actual postureinclination error to zero. Hence, each node compensating angle willfunction as a manipulated variable for manipulating the relativepositional relationship among the ground contact portions 10 so as toapproximate an actual posture inclination error to zero whileapproximating a floor reaction force moment about a desired total floorreaction force central point to a desired moment (a compensating totalfloor reaction force moment in this case).

As described above, the processing for correcting the desired groundcontact portion positions (specifically, the mutual relativerelationship among the positions) of the ground contact portions 10 isthe hierarchical compliance operation in the first embodiment. In thecompliance operation, for each node having child nodes, the manipulatedvariable (correction amount) of the relative relationship among thedesired ground contact portion positions (relative positionalrelationship) of the ground contact portions 10, which are thedescendant nodes of the node, is established for each of thecompensating angles θ14, θ23 and θ1423, and the manipulated variables(correction amounts) are combined so as to correct the mutual relativerelationship of the desired ground contact portion positions of theground contact portions 10. The desired ground contact portionposition/posture corrected as described above is referred to as acorrected desired ground contact portion position/posture.

In such a compliance operation, generally, if the aforesaid compensatingangles (compensating operation amounts) are not excessive, then theground contact region (a region of the ground contact surface whereinthe pressure is positive) remains unchanged even if the ground contactpressure distribution of each ground contact portion 10 changes. In thiscase, the compliance mechanism 42 attached to each ground contactportion 10 deforms in proportion to a compensating angle, and an actualfloor reaction force of each ground contact portion 10 based on theamount of the deformation is generated. As a result, the relationshipbetween the compensating angles and the changing amounts of the actualfloor reaction forces generated by the compensating angles has goodcharacteristics, namely, linear characteristics, shown below.

-   Characteristic 1) If only the compensating angles about the desired    floor reaction force central points of nodes that are not leaf nodes    are manipulated to move the desired ground contact portion positions    of the ground contact portions 10, then the translational force    components of the actual floor reaction forces of the ground contact    portions 10 that have been lowered increase, while the translational    force components of the actual floor reaction forces of the ground    contact portions 10 that have been raised decrease. At this time,    the actual floor reaction force moments about the corrected desired    floor reaction force central point of the ground contact portions 10    (leaf nodes) hardly change.-   Characteristic 2) If only an n-th ground contact portion    compensating angle is manipulated to rotate the desired posture of    the n-th ground contact portion, then the moment component of the    actual floor reaction force acting on the desired floor reaction    force central point of the n-th ground contact portion changes,    while the translational force component does not change much.-   Characteristic 3) If the compensating angle about the desired floor    reaction force central point of a node that is not a leaf node and    an n-th ground contact portion compensating angle are simultaneously    manipulated, then the change amount of the actual floor reaction    forces of the ground contact portions 10 will be equivalent to the    sum of the change amount observed when each of them is independently    manipulated.

FIG. 17 is a block diagram showing the functions of the hierarchicalcompliance operation determiner 114 in the present embodiment. Referringto the diagram, the processing of the hierarchical compliance operationdeterminer 114 will be explained in further detail.

The hierarchical compliance operation determiner 114 is equipped with,as its functional means, a compensating total floor reaction forcemoment distributor 114 a, compensating angle determiners 114 b, 114 cand 114d, a corrected desired ground contact portion position/posturecalculator 114 g, a deformation compensation amount calculator 114 n,and a corrected desired ground contact portion position/posture withdeformation compensation calculator 114 h.

The compensating total floor reaction force moment distributor 114 adistributes the aforesaid compensating total floor reaction force momentMdmd (Mdmdx, Mdmdy) to the 1423rd node compensating floor reaction forcemoment M1423dmd, the 14th node compensating floor reaction force momentM14dmd and the 23rd node compensating floor reaction force momentM23dmd.

The 1423rd node compensating floor reaction force moment M1423dmd is thedesired value of a moment to be generated about the desired 1423rd nodefloor reaction force central point (=desired total floor reaction forcecentral point (desired ZMP)) by the translational force components ofthe floor reaction forces of the ground contact portions 10(specifically, the first to the fourth ground contact portions) when the1423rd compensating angle θ1423 is manipulated.

The component of the 1423rd node compensating floor reaction forcemoment M1423dmd in the direction of the aforesaid vector V1423 (thecomponent about the axis in the V1423 direction) is described asM1423dmdv. The vector V1423 is the vector defined in the generalexplanation of the compliance operation of the hierarchical complianceoperation determiner 114 (refer to FIG. 16). If a vector orthogonal toV1423 and also orthogonal to the vertical direction is defined as U1423,then the component in the direction of U1423 of the 1423rd nodecompensating floor reaction force moment M1423dmd (the component aboutthe axis in the U1423 direction) M1423dmdu is set to zero in the presentembodiment. This is because the component of a floor reaction forcemoment in the U1423 direction cannot be generated even if the 1423rdnode compensating angle θ1423 is manipulated in the robot 1 of thepresent embodiment. Further, in the present embodiment, the rotation ofthe posture of the robot 1 about the vertical axis is not controlled;therefore, the component M1423dmdz of M1423dmd in the vertical directionis also set to zero.

The 14th node compensating floor reaction force moment M14dmd is thedesired value of a moment to be generated about the desired 14th nodefloor reaction force central point by the translational force componentsof the floor reaction forces of the ground contact portions 10(specifically, the first and the fourth ground contact portions) whenthe 14th compensating angle θ14 is manipulated.

The component of the 14th node compensating floor reaction force momentM14dmd in the direction of the vector V14 is described as M14dmdv. Thevector V14 is the vector defined in the general explanation of thecompliance operation of the hierarchical compliance operation determiner114 (refer to FIG. 15). If a vector orthogonal to V14 and alsoorthogonal to the vertical direction is defined as U14, then thecomponent M14dmdu in the direction of U14 of the 14th node compensatingfloor reaction force moment M14dmd is set to zero in the presentembodiment. This is because the component of a floor reaction forcemoment in the U14 direction cannot be generated even if the 14th nodecompensating angle θ14 is manipulated in the robot 1 of the presentembodiment. In the present embodiment, the component of M14dmd in thevertical direction is also set to zero.

The 23rd node compensating floor reaction force moment M23dmd is thedesired value of a moment to be generated about the desired 23rd nodefloor reaction force central point by the translational force componentsof the floor reaction forces of the ground contact portions 10(specifically, the second and the third ground contact portions) whenthe 23rd compensating angle θ23 is manipulated.

The component of the 23rd node compensating floor reaction force momentM23dmd in the direction of the vector V23 is described as M23dmdv. Thevector V23 is the vector defined in the general explanation of thecompliance operation of the hierarchical compliance operation determiner114 (refer to FIG. 15). If a vector orthogonal to V23 and alsoorthogonal to the vertical direction is defined as U23, then thecomponent M23dmdu in the direction of U23 of the 23rd node compensatingfloor reaction force moment M23dmd is set to zero in the presentembodiment. This is because the component of a floor reaction forcemoment in the U23 direction cannot be generated even if the 23rd nodecompensating angle θ23 is manipulated in the robot 1 of the presentembodiment. In the present embodiment, the component of M23dmd in thevertical direction is also set to zero.

The 1423rd node compensating floor reaction force moment M1423dmd, the14th node compensating floor reaction force moment M14dmd and the 23rdnode compensating floor reaction force moment M23dmd are determined, forexample, as follows.

On an arbitrary n-th node, when a desired n-th node floor reaction forcecentral point has been corrected while maintaining the horizontalcomponent of an n-th node compensating floor reaction force moment atzero so as to be equivalent to adding the n-th node compensating floorreaction force moment to the desired n-th node floor reaction forcecentral point, the desired n-th node floor reaction force central pointposition that has been corrected is referred to as a corrected desiredn-th node floor reaction force central point position.

The relationships between a corrected desired 1423rd node floor reactionforce central point Pmdfd (=Q1423mdfd), a corrected desired 14th nodefloor reaction force central point Q14mdfd, and a corrected desired 23rdnode floor reaction force central point Q23mdfd, and node compensatingfloor reaction force moments are shown in the following expressions 7 to9. The desired 1423rd node floor reaction force central point Q1423agrees with the total floor reaction force central point P, so that thecorrected desired 1423rd node floor reaction force central point Pmdfdmay be referred to as a corrected desired total floor reaction forcecentral point in some cases.M1423dmd=(Pmdfd−P)*Ftotalref  Expression 7M14dmd=(Q14mdfd−Q14)*F14ref  Expression 8M23dmd=(Q23mdfd−Q23)*F23ref  Expression 9

The difference between two points, such as (Pmdfd−P) in expression 7,means the difference in the positional vectors of the points. Further,Ftotalref, F14ref and F23ref denote the translational force componentsof the desired node floor reaction forces of the 1423rd node, the 14thnode and the 23rd node, respectively, as shown in FIG. 10.

To maintain high ground contact properties of the robot 1, controlshould not be carried out to bring the floor reaction force of theground contact portion 10 to be in contact with the ground excessivelyclose to zero or a negative value. Hence, the following conditions 1) to3) should be satisfied.

Corrected Node Existing Position Condition 1)

Pmdfd is not excessively close to an end of the segment Q14Q23 from thetotal floor reaction force central point P and exists on the segmentQ14Q23. The range in which the corrected desired 1423rd node floorreaction force central point (corrected desired total floor reactionforce central point) Pmdfd should exist is referred to as the existencepermissible range of the corrected desired 1423rd node floor reactionforce central point (corrected desired total floor reaction forcecentral point).

Corrected Node Existing Position Condition 2)

Q14mdfd is not excessively close to an end of the segment Q1Q4 from thedesired 14th node floor reaction force central point Q14 and exists onthe segment Q1Q4. The range in which the corrected desired 14th nodefloor reaction force central point Q14mdfd should exist is referred toas the existence permissible range of the corrected desired 14th nodefloor reaction force central point.

Corrected Node Existing Position Condition 3)

Q23mdfd is not excessively close to an end of the segment Q2Q3 from thedesired 23rd node floor reaction force central point Q23 and exists onthe segment Q2Q3. The range in which the corrected desired 23rd nodefloor reaction force central point Q23mdfd should exist is referred toas the existence permissible range of the corrected desired 23rd nodefloor reaction force central point.

Meanwhile, to obtain an appropriate posture restoring force actuallygenerated in the robot 1 (the force for restoring the inclination of thebody 24 to a desired body posture inclination), the resultant force ofthe 1423rd node compensating floor reaction force moment M1423dmd, the14th node compensating floor reaction force moment M14dmd and the 23rdnode compensating floor reaction force moment M23dmd shouldapproximately agree with the compensating total floor reaction forcemoment Mdmd. In other words, the following expression 10 should beapproximately satisfied.Mdmd=M1423dmd+M14dmd+M23dmd  Expression 10

In the present embodiment, therefore, the 1423rd node compensating floorreaction force moment M1423dmd, the 14th node compensating floorreaction force moment M14dmd and the 23rd node compensating floorreaction force moment M23dmd are determined according to expressions 11to 13 given below as long as the positions of the corrected desired nodefloor reaction force central points Pmdfd, Q14mdfd and Q23mdfddetermined by the above and the aforesaid expression 7, expression 8 andexpression 9 satisfy the aforesaid corrected node existence positionconditions 1), 2) and 3).M1423dmd=Mat1423*Mdmd*V1423  Expression 11M14dmd=Mat14*Mdmd*V14  Expression 12M23dmd=Mat23*Mdmd*V23  Expression 13where Mat1423, Mat14 and Mat23 denote gain matrixes (1-row, 3-columnmatrixes with a third element being zero), and these are set such thatM1423dmd, M14dmd and M23dmd determined according to expression 11,expression 12 and expression 13 satisfy expression 10.

More precisely, the gain matrixes Mat1423, Mat14 and Mat23 aredetermined such that a simultaneous equation composed of expression 10,expression 11, expression 12 and expression 13 identically holdsregardless of the value of Mdmd. The gain matrix for the simultaneousequation to identically hold is not uniquely determined, so that anappropriate gain matrix may be determined depending on, for example,which one of the corrected node existence position conditions 1), 2) and3) is especially important. Supplementally, the gain matrixes aredesirably changed continuously in order to continuously change thecompensating angles θ1423, θ14 and θ23. Further, the policy of settinggain matrixes may be changed according to whether the robot 1 isstanding upright or the difference in movement mode or the like.

If the 1423rd node compensating floor reaction force moment M1423dmd,the 14th node compensating floor reaction force moment M14dmd and the23rd node compensating floor reaction force moment M23dmd determinedaccording to expression 11, expression 12 and expression 13 do notsatisfy any one of the corrected node existence position conditions 1),2) and 3), then they are to be corrected to satisfy the conditions 1),2) and 3). More specifically, of the corrected desired n-th node floorreaction force central points (n=1423, 14, 23), the corrected desirednode floor reaction force central point that has exceeded the existencepermissible range is set on the boundary of the existence permissiblerange, and the remaining corrected desired node floor reaction forcecentral points are determined so as to satisfy expression 11, expression12 and expression 13 as much as possible (to minimize the absolute valueof the difference between a left side and a right side as much aspossible). Generally, however, the behavior of a controlled object doesnot considerably change even if a feedback amount slightly changes infeedback control; therefore, it is not required to strictly or forciblysatisfy expression 11, expression 12 and expression 13.

Thus, the compensating total floor reaction force moment distributor114a determines the node compensating floor reaction force momentsM1423dmd, M14dmd and M23dmd. FIG. 14 shows the examples of the nodecompensating floor reaction force moments M1423dmd, M14dmd and M23dmddetermined as described above. In the figure, Mdmd is the same as thatshown in FIG. 13 mentioned above.

Moreover, the compensating total floor reaction force moment distributor114 a determines the corrected desired ground contact portion floorreaction forces Fn_refmdfd (n=1, 2, 3, 4), which are the desired floorreaction forces of the ground contact portions 10 that are corrected byadding node compensating floor reaction force moments to desired groundcontact portion floor reaction forces, on the basis of the desiredground contact portion floor reaction forces, which are the desiredfloor reaction forces of the ground contact portions 10 (the desirednode floor reaction forces of leaf nodes) and the node compensatingfloor reaction force moments M1423dmd, M14dmd and M23dmd.

At this time, the corrected desired ground contact portion floorreaction forces Fn_refmdfd (n=1, 2, 3, 4) may be determined from thedesired floor reaction force central points Qn(n=1, 2, 3, 4), thecorrected total floor reaction force central points Pmdfd, the correcteddesired node floor reaction force central points Q14mdfd and Q23mdfd,and the desired total floor reaction forces Ftotalref of the groundcontact portions 10 by the same technique as the technique fordetermining deciding desired node floor reaction forces from the desiredfloor reaction force central points (the desired node floor reactionforce central points of leaf nodes) Qn(n=1, 2, 3, 4), the desired totalfloor reaction force central points P, the desired node floor reactionforce central points Q14 and Q23, and the desired total floor reactionforces Ftotalref of the ground contact portions 10. In other words, theweights of the nodes are determined according to the aforesaidexpressions 1 to 3 from the desired floor reaction force central pointsQn(n=1, 2, 3, 4), the corrected total floor reaction force centralpoints Pmdfd, the corrected desired node floor reaction force centralpoints Q14mdfd and Q23mdfd of the ground contact portions 10, and thecorrected desired ground contact portion floor reaction forcesFn_refmdfd (n=1, 2, 3, 4) may be determined according to the aforesaidexpression 4 by using the determined weight.

As is obvious from the above, generating a node compensating floorreaction force moment at a desired n-th node floor reaction forcecentral point of an n-th node having child nodes (correcting the momentcomponent of the desired floor reaction force acting on a desired n-thnode floor reaction force central point) is equivalent to correcting theweights of the child nodes of the n-th node.

The processing of the compensating total floor reaction force momentdistributor 114 a explained above is represented by the functional blockdiagram shown in FIG. 18. More specifically, based on the desired nodefloor reaction force central points Qn(n=1, 2, 3, 4, 14, 23), theexistence permissible ranges of the 14th node, the 23rd node and thecorrected desired node floor reaction force central points Qn_mdfd(n=14, 23, 1423) are determined according to the aforesaid correctednode existence position conditions 1), 2) and 3). Further, the correcteddesired node floor reaction force central points Qn_mdfd (n=14, 23,1423) and the node compensating floor reaction force momentsMn_dmd(n=14, 23, 1423) are determined on the basis of the compensatingtotal floor reaction force moment Mdmd, the desired floor reaction forcecentral points of nodes Qn(n=1, 2, 3, 4, 14, 23, 1423), the desiredfloor reaction forces Fn_ref(n=1, 2, 3, 4, 14, 23, 1423) and theaforesaid existence permissible ranges. In addition, the correcteddesired ground contact portion floor reaction forces Fn_refmdfd (n=1, 2,3, 4) are determined on the basis of the corrected desired node floorreaction force central points Qn_mdfd(n=14, 23, 1423), the desired floorreaction force central points Qn(n=1, 2, 3, 4) of the ground contactportions 10, and the desired total floor reaction force Ftotalref.

Next, the processing of the compensating angle determiner (θ1423determiner) 114 b out of the aforesaid compensating angle determiners114 b to 114 d will be explained with reference to the block diagram ofFIG. 19. If the translational force component (F1act+F4act) of theresultant force of the actual first node floor reaction force and theactual fourth node floor reaction force acts on a desired 14th nodefloor reaction force central point Q14 and the translational forcecomponent (F2act+F3act) of the resultant force of the actual second nodefloor reaction force and the actual third node floor reaction force actson a desired 23rd node floor reaction force central point Q23, a momentM1423 act generated about the desired total floor reaction force centralpoint P (the desired 1423rd node floor reaction force central point) bythe above translational force components is determined according toexpression 14 given below.M1423act=P _(—) Q14*(F1act+F4act)+P _(—) Q23*(F2act+F3act)  Expression14where P_Q14 denotes a vector whose start point is P and end point isQ14, and P_Q23 denotes a vector whose start point is P and end point isQ23.

In actuality, there will be hardly a problem if expression 15 givenbelow is used in place of expression 14 to calculate M1423 act.M1423act=P _(—) Q1*F1act+P _(—) Q2*F2act+P _(—) Q3*F3act+P _(—)Q4*F4act  Expression 15

The right side of expression 15 is an expression for calculating theactual total floor reaction force moment Mtotalact acting about thedesired total floor reaction force central point P from thetranslational force components Fn_act(n=1, 2, 3, 4) of the actual floorreaction forces of the first to the fourth nodes (leaf nodes).Supplementally, in expression 14, the actual floor reaction force momentacting about the desired 14th node floor reaction force central pointQ14 and the actual floor reaction force moment acting about the desired23rd node floor reaction force central point Q23 are subtracted from theactual total floor reaction force moment Mtotalact acting about thedesired total floor reaction force central point P.

In general, an actual n-th node floor reaction force moment Mn_act of anarbitrary n-th node, which is a leaf node, is the actual floor reactionforce moment of an n-th ground contact portion. Regarding an arbitraryn-th node, which is not a leaf node, the moment from the floor reactionforces of all child nodes thereof acting on the desired n-th node floorreaction force central point (the floor reaction forces of the childnodes here are, strictly speaking, the floor reaction forces acting onthe desired floor reaction force central points of the child nodes) isreferred to as an actual n-th node floor reaction force moment Mn_act.As in expression 14 and expression 15, there are two differentdefinitions of an actual n-th node floor reaction force moment. Oneincludes the actual floor reaction force moments of child nodes and theother does not include them; however, either of the definitions may beused. Especially when the responsiveness of feedback control forcontrolling the horizontal components of the actual floor reaction forcemoments of child nodes is high, the horizontal components of the actualfloor reaction force moments of the child nodes immediately converges tozero, so that the behavior of the control of the actual floor reactionforce of an n-th node does not change much when either of thedefinitions is used.

Expression 16 shown below is a general expression for calculating anactual n-th node floor reaction force moment corresponding to expression14, and expression 17 is a general expression for calculating an actualn-th node floor reaction force moment corresponding to expression 15. Inexpression 16 and expression 17, Qn_Qm is a vector whose start point isQn and end point is Qm. An actual floor reaction force moment Mm_act ofan m-th node, which is a leaf node, is an actual floor reaction forcemoment of each ground contact portion detected by an actual floorreaction force detector.

when mε {set of numbers of child nodes of n-th node},Mn _(—) act=Σ(Qn _(—) Qm*Fm _(—) act)  Expression 16when mε {set of numbers of leaf nodes, which are descendants of n-thnode},Mn _(—) act=Σ(Qn _(—) Qm*Fm_act+Mm _(—) act)  Expression 17

Σ in expressions 16 and 17 means the total sum regarding m.Supplementally, in the first embodiment, the actual floor reaction forcemoment about the desired floor reaction force central point of a leafnode (each ground contact portion) becomes zero; therefore, the rightside of the above expression 15 does not include a component of Mm_actof expression 17. In a robot in which the postures of ground contactportions can be controlled, Mm_act of expression 17 generally does notbecome zero.

The 1423rd node compensating angle θ1423 may generally be determined byfeedback control law or the like such that the difference betweenM1423act determined as described above and the 1423rd node compensatingfloor reaction force moment M1423dmd previously determined by thecompensating total floor reaction force moment distributor 114 a(M1423act−M1423dmd) is approximated to zero. For example, θ1423 may bedetermined by, for example, multiplying the difference by apredetermined gain matrix (tertiary diagonal matrix).

However, in the present embodiment, the 1423rd node compensating angleθ1423 about the axis in the direction of the aforesaid vector V1423 maybe determined, so that θ1423 may be determined on the basis of thedifference between a component M1423actv in the direction of the vectorV1423 of M1423act and a component M1423dmdv in the direction of thevector V1423 of M1423dmd. And, at this time, in the present embodiment,M1423actv and M1423dmdv are passed through a filter before determiningθ1423 on the basis of the difference therebetween in order to enhancethe responsiveness and stability of the control of floor reactionforces.

Specifically, the component M1423actv in the direction of the vectorV1423 of M1423 act determined as described above is extracted. This isobtained by the following expression 18 using a scalar productcalculation “·” of a vector.M1423actv=M1423act·V1423  Expression 18

The processing for calculating M1423actv as described above is executedby a calculator indicated by a reference numeral 114 k in FIG. 19.

Next, the aforesaid M1423actv is passed through a low-pass filter 114 ito obtain M1423actvfilt. Furthermore, the component M1423dmdv in theV1423 direction of the aforesaid 1423rd node compensating floor reactionforce moment M1423dmd is passed through a compensating filter 114 j toobtain M1423dmdvfilt. Incidentally, M1423dmdv is determined by thescalar product calculation of M1423dmd and V1423, as with the aforesaidexpression 18. Then, the result obtained by subtracting M1423dmdvfiltfrom M1423actvfilt provides a component M1423errv in the direction of anerror moment V1423.

The aforesaid compensating filter 114 j improves the frequency responsecharacteristic of transfer functions from M1423dmdv to an actual totalfloor reaction force moment in a control device.

Lastly, the aforesaid 1423rd node compensating angle θ1423 is obtainedby the calculation of the feedback control law (proportional control lawin this case) of the following expression 19. Here, K1423 denotes acontrol gain, and it is normally set to a positive value.θ1423=K1423*M1423errv  Expression 19This means that the component M1423errv in direction of the error momentV1423 is multiplied by the control gain K1423 to obtain the 1423rd nodecompensating angle θ1423.

The processing of the compensating angle determiner (θ14 determiner) 114c in FIG. 17 will now be explained with reference to FIG. 20. Thetranslational force component Flact of the actual first node floorreaction force (the actual floor reaction force of the first groundcontact portion 10) acts on a desired first node floor reaction forcecentral point Q1 and the translational force component F4act of theactual fourth node floor reaction force (the actual floor reaction forceof the fourth ground contact portion 10) acts on a desired fourth nodefloor reaction force central point Q4, a moment M14act generated aboutthe desired 14th node floor reaction force central point Q14 by theabove translational force components is determined according to theaforesaid expression 16, which is a general expression.

More specifically, it is determined according to the followingexpression 20.M14act=Q14_(—) Q1*F1act+Q14_(—) Q4*F4act  Expression 20where Q14_Q1 denotes a vector whose start point is Q14 and end point isQ1, and Q14_Q4 denotes a vector whose start point is Q14 and end pointis Q4.

In actuality, for the reason described previously, there will be hardlya problem if the following expression 21 (expression substantiating theabove expression 17) is used in place of expression 20.M14act=Q14_(—) Q1*F1act+Q14_(—) Q4*F4act+M1act+M4act  Expression 21where M1act denotes an actual first node floor reaction force moment andM4act denotes an actual fourth node floor reaction force moment. In thefirst embodiment, the distal portions of the legs #1 to #4 are providedwith free joints (spherical joints 12), so that M1act and M4act arezero.

Expression 21 is an expression for calculating the moment acting aboutthe desired 14th node floor reaction force central point generated bythe resultant force of the actual floor reaction forces of all leafnodes of the 14th node. Incidentally, expression 20 subtracts the actualfirst node floor reaction force moment and the actual fourth node floorreaction force moment from the moment acting about the desired 14thfloor reaction force central point generated by the resultant force ofthe actual floor reaction forces of all leaf nodes of the 14th node.

The 14th node compensating angle θ14 may be generally determined by thefeedback control law or the like so as to approximate the differencebetween M14act determined as described above and the 14th nodecompensating floor reaction force moment M1423dmd previously determinedby the compensating total floor reaction force moment distributor 114 a(M14act−M14dmd) to zero.

In the present embodiment, however, for the same reason as that in thecase of θ1423, θ14 is determined on the basis of the difference betweenthe component M14actv of M14act in the aforesaid direction of the vectorV14 and the component M14dmdv of M14dmd in the direction of the vectorV14, which have been respectively passed through a filter.

More specifically, the component M14actv of M14act in the direction ofthe vector V14 determined as described above is extracted. This isobtained by expression 22 that uses the scalar product calculation ofvectors.M14actv=M14act·V14  Expression 22

The processing for calculating M14actv as described above is carried outby a calculator indicated by a reference character 114 k′ in FIG. 20.

Next, the aforesaid M14actv is passed through a low-pass filter 114 i′to obtain M14actvfilt. Further, the component M14dmdv of the aforesaid14th node compensating floor reaction force moment in the direction ofV14 is passed through a compensating filter 114 j′ to obtainM14dmdvfilt. Incidentally, M14dmdv is determined by the scalar productcalculation of M14dmd and V14. Then, M14dmdvfilt is subtracted fromM14actvfilt to obtain a component M14errv in the direction of an errormoment V14. Incidentally, the compensating filter 114 j′ improves thefrequency response characteristics of transfer functions from M14dmdv toactual total floor reaction force moments in a control device.

Lastly, the aforesaid 14th node compensating angle θ14 is obtained bythe calculation of the feedback control law (proportional control law inthis case) of expression 23 given below. Here, K14 denotes a controlgain, which is normally set to a positive value.θ14=K14*M14errv  Expression 23

The processing of the compensating angle determiner (θ23 determiner) 114d in FIG. 17 is the same as the processing of the 14th node compensatingangle θ14 determiner 114 c; therefore, detailed explanation thereof willbe omitted here. The following provides an overview of the processing. Acomponent M23actv of a moment M23act in the direction of a vector V23,the moment being generated about a desired 23rd node floor reactionforce central point Q23 by the actual floor reaction forces of the childnodes of a 23rd node, is calculated on the basis of the aforesaidexpression 16 or 17. Then, the 23rd node compensating angle θ23 iscalculated by the arithmetic processing of the feedback control law(proportional control law) from a component M23errv in the direction ofthe error moment V23 obtained by subtracting M23actvfilt, which isobtained by passing the above M23actv through a low-pass filter, fromM23dmdvfilt obtained by passing M23dmdv of the aforesaid 23rd nodecompensating floor reaction force moment M23dmd in the direction of V23through a compensating filter.

The processing of the compensating angle determiners 114 b to 114 ddetermines the sets of the node compensating angles θ1423, θ14 and θ23such that the horizontal components of the actual moments Mact acting onthe desired total floor reaction force central points P are approximatedto the compensating total floor reaction force moment Mdmd. In thepresent embodiment, θ1423 has been determined by the aforesaidexpression 19; alternatively, however, it may be determined on the basisof the difference between M1423act+M14act+M23act (the total sum ofactual node floor reaction force moments) and M1423dmd or the differencebetween the total sum of actual node floor reaction force moments andM1423dmd, which have been respectively passed through a filter, in placeof M1423err of the right side of expression 19.

Supplementally, in the present embodiment, a node compensating floorreaction force moment has been determined without changing the desirednode floor reaction force central point Qn of each node, the Qn beingthe point of action. Then, the difference between the node floorreaction force compensating moment (more precisely, the resultant forceof the node compensating floor reaction force moment and the momentcomponent of the desired node floor reaction force having Qn as itspoint of action) and an actual node floor reaction force moment havingQn as its point of action has been used as a control amount to determinea node compensating angle such that the control amount is approximatedto zero. Instead of determining the node compensating angles asdescribed above, the node compensating angles θ1423, θ14 and θ23 may bedetermined as follows. On each n-th node (n=14, 23, 1423) having childnodes, a floor reaction force central point that causes the horizontalcomponent of the moment of the actual node floor reaction force of then-th node (the resultant force of the actual node floor reaction forcesof all child nodes of the n-th node) to become zero is determined as anactual n-th node floor reaction force central point. Alternatively, afloor reaction force central point that causes the horizontal componentof a moment, which is obtained by subtracting the moment acting on thedesired node floor reaction force central point of each child node dueto the actual node floor reaction force of the child node from themoment of the actual node floor reaction force of an n-th node (n=14,23, 1423), to become zero is determined as an actual n-th node floorreaction force central point. For example, the actual 14th node floorreaction force central point related to the 14th node is determined as apoint obtained by shifting, on the segment Q1Q4, the desired 14th nodefloor reaction force central point by the value obtained by dividing thehorizontal component of M14act determined by the aforesaid expression 16or 17 by the vertical component of the resultant force of F1act andF4act (the translational force component of an actual 14th node floorreaction force). The same applies to the node floor reaction forcecentral points of the 23rd node and the 1423rd node. Then, thedifference between the desired node floor reaction force central pointof an n-th node (n=14, 23, 1423) and the actual n-th node floor reactionforce central point determined as described above, or the differencebetween the values obtained by passing each of the above node floorreaction force central points through a filter is defined as a controlamount, and the node compensating angles θ1423, θ14 and θ23 aredetermined on the basis of the control amount such that the controlamount is approximated to zero (e.g., θ1423, θ14 and θ23 are determinedby multiplying the control amount by a certain gain).

The corrected desired ground contact portion position/posture calculator114 g in FIG. 17 obtains corrected desired ground contact portionposition/posture by correcting the desired ground contact portionposition/posture, which are the desired position and the desired postureof each ground contact portion 10, according to a technique forcorrecting the desired ground contact portion position/posture of theaforesaid hierarchical compliance operation (the technique explainedwith reference to FIG. 15 and FIG. 16) on the basis of the 1423rd nodecompensating angle θ1423, the 14th node compensating angle θ14, and the23rd node compensating angle θ23. However, in the present embodiment,the distal portions of the legs #1 to #4 are provided with free joints(spherical joints 12), and the posture of each ground contact portion 10cannot be intentionally changed, so that the corrected desired groundcontact portion position/posture actually means the corrected desiredground contact portion position.

FIG. 21 is a functional block diagram showing the processing of theaforesaid deformation compensation amount calculator 114 n in FIG. 17.As shown in FIG. 21, the deformation compensation amount calculator 114n determines deformation amounts En_mdfd(n=1, 2, 3, 4), which are thedeformation amounts of the legs #1 to #4 (the deformation amounts of thelink mechanisms of the legs and the compliance mechanism 42), thedeformation being expected to occur due to the corrected desired groundcontact portion floor reaction forces Fn_refmdfd (n=1, 2, 3, 4) of theground contact portions 10 that are output from the compensating totalfloor reaction force moment distributor 114 a (the desired groundcontact portion floor reaction forces that have been corrected by addingthe 1423rd node compensating floor reaction force moment M1423dmd, the14th node compensating floor reaction force moment M14dmd and the 23rdnode compensating floor reaction force moment M23dmd to desired groundcontact portion floor reaction forces). The deformation amountsEn_mdfd(n=1, 2, 3, 4) are determined by using a mechanism compliancemodel showing the relationship between the forces (or floor reactionforces) acting on the legs and the deformations of the legs. Then, thedeformation compensation amount calculator 114 n determines deformationcompensation amounts En_cmpn(n=1, 2, 3, 4) for canceling the deformationamounts En_mdfd(n=1, 2, 3, 4). The deformation compensation amountEn_cmpn is determined by multiplying each deformation amount En_mdfd by(−1).

The corrected desired ground contact portion position/posture withdeformation compensation calculator 114h in FIG. 17 adds a furthercorrection to the corrected desired ground contact portionposition/posture of each ground contact portion 10 (the position/posturedetermined by the aforesaid corrected desired ground contact portionposition/posture calculator 114 g) so as to cancel the calculateddeformation amount En_mdfd, thereby obtaining the corrected desiredground contact portion position/posture with deformation compensation ofeach ground contact portion 10. The corrected desired ground contactportion positions/postures with deformation compensation are determinedby adding individually corresponding deformation compensation amountsEn_cmpn to the corrected desired ground contact portion position/postureof the ground contact portions 10.

For example, if it is calculated that a corrected desired ground contactportion floor reaction force causes the compliance mechanism 42 or thelike of a leg corresponding to a certain ground contact portion 10 tocontract by z in the vertical direction, then the corrected desiredground contact portion floor reaction force is corrected so as to lowerthe desired position of the ground contact portion 10 by z. Morespecifically, the corrected desired ground contact portionposition/posture with deformation compensation is calculated such thatthe position/posture when the ground contact surface (bottom surface) ofthe ground contact portion 10 after deformation compensation is deformedunder a desired ground contact portion floor reaction force agrees withthe desired position/posture of the ground contact surface of the groundcontact portion before the deformation compensation. Incidentally,detailed explanation thereof is explained in detail in JapaneseUnexamined Patent Application Publication No. H10-277969. In the robot 1of the present embodiment, the postures of the ground contact portions10 cannot be controlled, so that the corrected desired ground contactportion position/posture with deformation compensation calculator 114 hactually corrects the corrected desired ground contact portion positionsof the ground contact portions 10.

The deformation compensation described above is implemented by controlfor canceling in a feed-forward manner the shifts in actual groundcontact portion position/posture caused by a deformation of thecompliance mechanism 42 or the like. In comparison with a case where nosuch control is carried out, it is possible to enable the robot 1 totravel in a gait that is further closer to a desired gait.

The above has given the details of the processing of the hierarchicalcompliance operation determiner 114.

Based on the above, resuming the explanation of the flowchart in FIG. 9,compensating angles are determined as described above in S34. FIG. 22 isa flowchart showing a generalized subroutine of the processing fordetermining the compensating angles.

To explain with reference to the figure, first, the translational forcecomponents Fn_act(n=1, 2, 3, 4, 14, 23, 1423) of actual n-th node floorreaction forces are calculated on the basis of the actual floor reactionforces of the ground contact portions 10 (the actual floor reactionforces obtained by the aforesaid actual floor reaction force detector108). In this case, the translational force components Fn_act(n=1, 2, 3,4) of the actual node floor reaction forces of the leaf nodes are thetranslational force components of the detected values of the actualfloor reaction forces of the ground contact portions 10 by the aforesaidactual floor reaction force detector 108. Further, the translationalforce component Fn_act(n=14, 23, 1423) of the actual node floor reactionforce of a node that is not a leaf node is the translational forcecomponent of the resultant force of the actual node floor reactionforces of the child nodes of the node, as described above.

Subsequently, the processing proceeds to S102 to calculate an actualn-th node floor reaction force moment Mn_act (n=1, 2, 3, 4, 14, 23,1423). In the robot 1 of the first embodiment, the actual node floorreaction force moment Mn_act (n=1, 2, 3, 4) of each leaf node is zero.Further, the actual node floor reaction force moment Mn_act(n=14, 23,1423) of each node that is not a leaf node is calculated on the basis ofthe aforesaid expression 16 or expression 17 from Fn_act(n=1, 2, 3, 4)obtained in S100 and the desired node floor reaction force central pointQn(n=1, 2, 3, 4, 14, 23, 1423).

Subsequently, the processing proceeds to S104 wherein the n-th nodecompensating floor reaction force moment Mn_dmd (n=14, 23, 1423) isdetermined on the basis of the compensating total floor reaction forcemoment Mdmd determined by the aforesaid posture stabilization controlcalculator 104. This processing is carried out as described above by theaforesaid compensating total floor reaction force moment distributor 114a.

Subsequently, the processing proceeds to S106 to determine the vectorsV1423, V14 and V23 defined in the explanation of the hierarchicalcompliance operation and vectors U1423, U14 and U23 orthogonal thereto.In the first embodiment, however, the floor reaction force moments inthe directions of the vectors U1423, U14 and U23 cannot be generated;therefore, it is unnecessary to determine U1423, U14 and U23.

Supplementally, as in the second embodiment to be described hereinafter,if the number of child nodes of an n-th node is 3 or more, then Vn maybe taken in any direction as long as it does not suddenly changetimewise; hence, the direction of Vn may be appropriately determined by,for example, setting it to the direction of the X axis of a supportingleg coordinate system or the direction of the body of a robot. Further,Un is to be orthogonal to Vn.

Subsequently, the processing proceeds to S108 to extract a componentMn_actv in the Vn direction and a component Mn_actu in the Un directionof an actual n-th node floor reaction force moment Mn_act(n=14, 23,1423). This may be accomplished by carrying out the scalar productcalculation of Mn_act and Vn and Un. Incidentally, if the number ofchild nodes of an arbitrary n-th node that is not a leaf node is two orless, as in the first embodiment, then the component Mn_actu in the Undirection is zero. Hence, it is unnecessary to determine Mn_actu.

Subsequently, the processing proceeds to S110 to extract a componentMn_dmdv in the Vn direction and a component Mn_dmdu in the Un directionof an n-th node compensating floor reaction force moment Mn_dmd(n=14,23, 1423). This may be accomplished by carrying out the scalar productcalculation of Mn_dmd and Vn and Un. Incidentally, if the number ofchild nodes of an arbitrary n-th node that is not a leaf node is two orless, as in the first embodiment, then the component Mn_dmdu in the Undirection of the n-th node compensating floor reaction force momentMn_dmd is set to zero. Alternatively, it is unnecessary to determineMn_dmdu.

Subsequently, the processing proceeds to S112 to determine a Vncomponent of an n-th node compensating angle θn by multiplying thedifference between a value obtained by passing Mn_actv through a filterand a value obtained by passing Mn_dmdv through a filter by a gain Kn(more generally, according to the feedback control law from thedifference). In the first embodiment, this processing is carried out asdescribed above on the 14th node, the 23rd node and the 1423rd node bythe aforesaid compensating angle determiners 114 b, 114 c and 114 d.

Subsequently, the processing proceeds to S114 to determine a Uncomponent of an n-th node compensating angle θn by multiplying thedifference between a value obtained by passing Mn_actu through a filterand a value obtained by passing Mn_dmdu through a filter by the gain Kn(more generally, according to the feedback control law from thedifference). However, if the number of child nodes of an arbitrary n-thnode that is not a leaf node is two or less, as in the first embodiment,then an n-th node compensating angle U component is set to zero.Alternatively, it is unnecessary to carry out the processing of S114.

The above is the subroutine processing of S34 in FIG. 9. Supplementally,the processing of S106 to S114 may be regarded as the processing fordetermining an n-th node compensating angle such that the actual n-thnode floor reaction force moment acting on a desired n-th node floorreaction force central point converges to an n-th node compensatingfloor reaction force moment (more precisely, the resultant force of then-th node compensating floor reaction force moment and the desired nodefloor reaction force moment acting on the desired n-th node floorreaction force central point).

Subsequently, the processing proceeds to S36 of the flowchart of FIG. 9to calculate the aforesaid deformation compensation amount. Thisprocessing is carried out by the aforesaid deformation compensationamount calculator 114 n as described above.

Subsequently, the processing proceeds to S38 to correct a desired groundcontact portion position/posture on the basis of the compensating angledetermined in S34 and to further correct it on the basis of thedeformation compensation amount determined in S36, thereby obtaining thecorrected desired ground contact portion position/posture withdeformation compensation of each ground contact portion 10. In the firstembodiment, the corrected desired ground contact portion positions ofthe ground contact portions 10 are determined on the basis of thecompensating angles θ1423, θ14 and θ23 as described above (as explainedwith reference to FIG. 15 and FIG. 16) by the corrected desired groundcontact portion position/posture calculator 114 g. Then, the determinedcorrected desired ground contact portion positions are further correctedby the corrected desired ground contact portion position/posture withdeformation compensation calculator 114 h on the basis of the aforesaiddeformation compensation amount En_cmpn(n=1, 2, 3, 4), as describedabove, thereby obtaining the corrected desired ground contact portionpositions with deformation compensations of the ground contact portions10.

The processing from S32 to S38 in FIG. 9 explained above is theprocessing of the hierarchical compliance operation determiner 114.

Subsequently, the processing proceeds to S40 to calculate the jointdisplacement command of the robot 1 from the desired bodyposition/posture and the corrected ground contact portionposition/posture with deformation compensation (the corrected groundcontact portion position with a deformation compensation in the firstembodiment). This processing is carried out as described above by theaforesaid robot geometric model 110.

Subsequently, the processing proceeds to S42 to make an actual jointdisplacement follow the joint displacement command. This follow-upcontrol (servo control) is carried out by the aforesaid displacementcontroller 112.

Subsequently, the processing proceeds to S44 to update time by Δt (thearithmetic processing cycle of the control device 50), and returns toS14 to repeat the processing described above.

Owing to the hierarchical compliance operation as in the firstembodiment explained above, the controls of node floor reaction forceshardly interfere with each other, thus allowing the node floor reactionforces to be controlled easily and properly also in a mobile robothaving three or more ground contact portions. This restrains controlinterference and also restrains the actual floor reaction force of eachnode from deviating from a desirable value or from vibrating. Thus, evenif a floor surface not only has an undulation or tilt over a large areathereof but also unexpected changes in the floor configuration,including local projections or depressions or slopes, floor reactionforces acting on a legged mobile robot can be properly controlledwithout being influenced by them much. Moreover, control for stabilizingthe posture of a mobile robot can be easily accomplished and the landingshocks to the mobile robot can be reduced, thus allowing the groundcontact properties of the mobile robot to be enhanced and alsopreventing the mobile robot from slipping or spinning when it travels.Furthermore, the loads of the actuators of the mobile robot can bereduced. Hence, the floor reaction forces of ground contact portions canbe properly controlled, so that high posture stability can be obtained.

To supplement the advantages contributing to the stabilization of theposture of a robot, when the entire robot inclines from a state in whichthe robot is walking as expected on an expected floor surface, therelationship between an inclination angle error θberr (θberrx, θberry)and an increasing amount ΔM of the moment horizontal component about adesired total floor reaction force central point generated in responsethereto is preferably a proportional relationship. If not, it is stillpreferred that expression 24 given below holds for a certain rotationalmatrix T and a certain diagonal matrix diag(a,b). Incidentally, T,diag(a,b) are secondary square matrixes.T*ΔM=diag(a,b)*T*θberr  Expression 24

If these relationships are not satisfied, then the inclination angleerror θberr will not linearly converge when the robot restores itsposture from the inclined state, and a precession movement may occur.For instance, in a state wherein the body of a robot inclines forward,the force (restoring force) to fall sideways other than the restoringforce for returning backward excessively acts, and the inclination angleerror θberr does not linearly returns to zero. Instead, a restoringforce acts toward back sideways and the inclination angle error θberrspirally converges to zero.

For the same reason as that described above, in the compliance control,the relationship between a changing rate dθberr/dt of the inclinationangle error θberr of the entire robot and an increasing amount ΔM_d ofthe moment generated in response thereto is also preferably aproportional relationship. If not, it is still preferred that expression25 given below holds for a certain rotational matrix T and a certaindiagonal matrix diag(e,f). Incidentally, T, diag(e,f) are secondarysquare matrixes.T*ΔM _(—) d=diag(e,f)*T*dθberr/dt  Expression 25

In general, if compliance control is independently carried out on eachleg of a robot, these relationships may not be satisfied and aprecession movement may take place. In the hierarchical compliancecontrol shown in the first embodiment, the relationships of theaforesaid expressions 24 and 25 can be satisfied, so that theconvergence of the posture control of the robot is high and it ispossible to prevent oscillation or vibration.

In the aforesaid first embodiment, the desired ground contact portionpositions have been amended (corrected) so as to rotationally move aboutnode floor reaction force central points without changing the posturesof the ground contact portions (without controlling the postures),thereby correcting the relative heights of the ground contact portions10 from the relative heights of the desired ground contact portionpositions. Alternatively, however, only the heights of the groundcontact portions 10 may be corrected by moving the desired groundcontact portion positions only in the vertical direction. Specifically,a desired ground contact portion position is corrected according to thefollowing procedure.

First, by the following expressions 26 and 27, a vertical positioncorrection amount Z14 of a desired 14th node floor reaction forcecentral point and a vertical position correction amount Z23 of a desired23rd node floor reaction force central point are determined.Z14=−PQ14*θ1423  Expression 26Z23=PQ23*θ1423  Expression 27

Here, note that the value determined by the aforesaid expression 19 issubstituted into θ1423.

Next, the vertical position correction amounts Zn(n=1, 2, 3, 4) of thedesired floor reaction force central points of the ground contactportions (the desired node floor reaction force central points of leafnodes) are determined according to the following expressions.Z1=−Q14Q1*θ14+Z14  Expression 28Z4=Q14Q4*θ14+Z14  Expression 29Z2=−Q23Q2*θ23+Z23  Expression 30Z3=Q23Q3*θ23+Z23  Expression 31

Here, note that the value determined by the aforesaid expression 23 issubstituted into θ14, and the value determined in the same manner asthat for θ14 is substituted into θ23.

The corrected desired ground contact portion positions are obtained byadding Z1, Z2, Z3 and Z4 determined as described above to the desiredground contact portion positions in the vertical direction.

Further, in the aforesaid first embodiment, the ground contact portions10 have been hierarchized as shown in the aforesaid FIG. 6; however, thehierarchical structure does not necessarily have to be decided to be onehierarchical structure beforehand. For example, the hierarchicalstructure may be changed according to the travel mode (the motion modeof legs when traveling) of the robot 1, including trotting or galloping.For instance, the ground contact portions 10 may be hierarchized asshown in FIG. 23. FIG. 23 illustrates, as in the aforesaid FIG. 3( b),an example wherein the pair of the first ground contact portion 10 andthe second ground contact portion 10 is defined as a 12th node and thepair of the third ground contact portion 10 and the fourth groundcontact portion 10 is defined as a 34th node in a period during whichall legs #1 to #4 of the robot 1 are all supporting legs. In thisexample, the desired node floor reaction force central points Qn(n=1, 2,3, 4, 12, 34, 1234) of the nodes are set as illustrated. With thisarrangement, the aforesaid compliance operation and the estimation of afloor configuration, which will be discussed later, can be performedmore accurately in some cases.

SECOND EMBODIMENT

The following will explain a second embodiment of the present inventionwith reference to the aforesaid FIG. 1 and FIG. 24 to FIG. 34. Referringto FIG. 1, the explanation will be focused on the aspects of a robot 1of the second embodiment that are different from the robot of the firstembodiment. The robot 1 of the second embodiment is provided with, inaddition to the first to the fourth legs #1 to #4, a fifth leg #5 and asixth leg #6 having the same structures as those of the legs #1 to #4.This means that the robot 1 of the second embodiment is a six-leggedrobot. The fifth leg #5 is behind the third leg #3 and extended from theright side of the body 24 of the robot 1, and the sixth leg #6 is behindthe fourth leg #4 and extended from the left side of the body 24 of therobot 1. The rest of the mechanical structure of the robot 1 is the sameas that of the first embodiment, so that the like reference numerals asthose of the first embodiment will be assigned and the explanationthereof will be omitted.

The basic concept of the technique of the hierarchical compliancecontrol of the robot 1 (six-legged robot) of the second embodiment isthe same as that of the first embodiment. However, the hierarchicalcompliance control of the second embodiment differs from that of thefirst embodiment in that there is a node having three child nodes andcontrol processing is expanded to handle them.

The following will explain the control processing of the robot 1 of thesecond embodiment, focusing mainly on the different aspects. Regardingthe reference numerals and terms used in the explanation of the secondembodiment, the same reference numerals and terms as those in the firstembodiment will be used for those that have the equivalent meanings asthose in the first embodiment, and detailed explanation thereof will beomitted.

FIG. 24 is a diagram for explaining the hierarchical structure in thesecond embodiment, and it is a diagram corresponding to FIG. 3( b) inthe first embodiment. As shown in FIG. 24, in the second embodiment, thesix individual ground contact portions 10 are defined as leaf nodes (thefirst to the sixth nodes), and the set of all the six ground contactportions 10 is defined as a root node (the 145236th node). In addition,the set of the first, the fourth, and the fifth ground contact portions10, which are the ground contact portions of the first leg #1, thefourth leg #4, and the fifth leg #5 is defined as a 145th node, and theset of the second, the third, and the sixth ground contact portions 10,which are the ground contact portions of the second leg #2, the thirdleg #3, and the sixth leg #6 is defined as a 236th node. In other words,the 145th node is an intermediate node having the first, the fourth, andthe fifth nodes (three leaf nodes) as child nodes, and the 236th node isan intermediate node having the second, the third, and the sixth node(three leaf nodes) as child nodes.

In the second embodiment, for example, the set of the three groundcontact portions 10 of the 145th node and the set of the three groundcontact portions 10 of the 236th node are alternately lifted and landedto move the robot 1. FIG. 24 shows the state wherein both sets arelanded (the state wherein both sets are supporting legs). The triangleswith the reference marks Qn(n=1, 2, 3, 4, 5, 6) in the figure correspondto the positions of the ground contact portions 10.

Incidentally, Qn(n=1, 2, 3, 4, 5, 6) denotes the desired floor reactionforce central point of each of the first to the sixth ground contactportions 10, which are leaf nodes (desired node floor reaction forcecentral points), Q145 and Q236 denote the desired floor reaction forcecentral points of the 145th node and the 236th node, respectively(desired node floor reaction force central points), and P denotes thedesired total floor reaction force central point (=desired ZMP) as thedesired floor reaction force central point (desired node floor reactionforce central point) Q145236 of the root node (the 145236th node). Aswith the first embodiment, Qn(n=1, 2, 3, 4, 5, 6) agrees with thecentral point of each corresponding ground contact portion 10.

The overall functional construction of a control device 50 in the secondembodiment is the same as that shown in FIG. 2 explained in conjunctionwith the aforesaid first embodiment.

In this case, a gait generating device 100 in the second embodimentdetermines and outputs desired motion trajectories (a desired groundcontact portion trajectory and a desired body position/posturetrajectory) of the robot 1 and desired floor reaction force trajectories(a desired total floor reaction force central point trajectory and adesired total floor reaction force trajectory), as with that in thefirst embodiment. In the second embodiment, however, the desired groundcontact portion trajectory is the trajectory of the desired groundcontact portion position of each of the six ground contact portions 10.If the ground contact portions are provided such that their postures arecontrollable, then the trajectory of desired ground contact portionpostures is also included in the desired ground contact portiontrajectory. Further, the desired total floor reaction force centralpoint trajectories are determined such that they continuously move whileexisting in a range in which ZMPs may exist at positions not excessivelyclose to the boundaries of the range (e.g., at an approximately centralposition of the range wherein a ZMP may exist) according to the motionmodes of the legs #1 to #6 (especially the positions at which supportinglegs are expected to land) in the second embodiment.

A desired floor reaction force distributor 102 in the second embodimentdetermines desired node floor reaction force central points, the weightsof nodes, and desired node floor reaction forces such that the followingconditions A′) to G′) are satisfied, as with the first embodiment.

-   A′) The desired node floor reaction force central point Qn(n=1, 2,    3, 4, 5, 6) of each leaf node agrees with the central point of the    ground contact portion 10 corresponding to the leaf node. More    generally, Qn(n=1, 2, 3, 4, 5, 6) is determined according to a    desired gait (a desired motion, such as a desired ground contact    portion trajectory). For example, if the desired floor reaction    force central point of each ground contact portion 10 is determined    by the gait generating device 100, then the desired node floor    reaction force central point Qn(n=1, 2, 3, 4, 5, 6) may be    determined on the basis of the desired floor reaction force    determined by the gait generating device 100.-   B′) The desired node floor reaction force central point of the root    node agrees with a desired total floor reaction force central point    P.-   C′) The desired node floor reaction force central point Qn of an    arbitrary n-th node (n=145, 236, 145236) having child nodes will be    a weighted average point of the desired node floor reaction force    central points of the child nodes of the n-th node. More    specifically, a desired node floor reaction force central point Q145    of a 145th node provides the internally dividing point of a triangle    having, as its apexes, desired node floor reaction force central    points Q1, Q4 and Q5 of the first, the fourth and the fifth nodes,    which are the child nodes (leaf nodes) thereof (points on a boundary    of the triangle being included), and a desired node floor reaction    force central point Q236 of a 236th node provides the internally    dividing point of a triangle having, as its apexes, desired node    floor reaction force central points Q2, Q3 and Q6 of the second, the    third and the sixth nodes, which are the child nodes (leaf nodes)    thereof (points on a boundary of the triangle being included).    Further, a desired node floor reaction force central point Q145236    (=desired total floor reaction force central point P) of a 145236th    node (root node) provides an internally dividing point of a segment    connecting the desired node floor reaction force central points Q145    and Q236 of the 145th node and the 236th node, which are the child    nodes (intermediate nodes) thereof.

D′) The desired node floor reaction force Fn of an arbitrary n-th node(n=145, 236, 145236) having child nodes agrees with the sum (resultantforce) of the desired node floor reaction forces of all child nodes ofthe n-th node, and the desired node floor reaction force F145236 of theroot node (the 145236th node) agrees with the desired total floorreaction force Ftotalref. Alternatively, the desired node floor reactionforce Fn(n=1, 2, 3, 4, 5, 6, 145, 236, 145236) of each node has therelationship of the aforesaid expression 4 a or 4 b with the weight ofeach node.

-   E′) The desired node floor reaction force of a leaf node associated    with the ground contact portion 10 not in contact with the ground is    zero.-   F′) The desired node floor reaction force central point, the weight,    and the desired node floor reaction force of each node (n-th node    (n=1, 2, 3, 4, 145, 236, 145236)) continuously change.-   G′) The weight of a leaf node corresponding to the ground contact    portion 10 in a no-contact-with-ground state or the weight of any    one of the ancestor nodes of the leaf node is zero.

The weight of the root node has been set to “1” for the sake ofconvenience, as with the aforesaid first embodiment.

The processing of a posture error calculator 103 and a posturestabilization control calculator 104 in the second embodiment is thesame as that in the first embodiment; a compensating total floorreaction force moment Mdmd (Mdmdx, Mdmdy) is determined as explained inthe first embodiment.

An actual floor reaction force detector 108 in the second embodiment, asthat in the first embodiment, detects the actual floor reaction forcesacting on the ground contact portions 10 (the six ground contactportions in the present embodiment) from the outputs of six-axis forcesensors 34 provided on the legs #1 to #6 and converts them into actualfloor reaction forces represented by a supporting leg coordinate system(the global coordinate system shown in FIG. 1, which is fixed to afloor).

Primarily on the basis of the final desired trajectories of groundcontact portion positions and desired body position/posture, a robotgeometric model (inverse kinematics calculator) 110 in the secondembodiment calculates joint displacement commands of the robot 1 thatsatisfy them by inverse kinematics calculation, as that in the firstembodiment.

Further, a displacement controller 112 in the second embodiment controlsactuators (not shown) of the joints 14 and 15 of the robot 1 so as tomake actual joint displacements follow the aforesaid joint displacementcommands, as that in the first embodiment.

Further, a hierarchical compliance operation determiner 114 in thesecond embodiment corrects the desired ground contact portiontrajectories of the ground contact portions 10 so as to satisfy theaforesaid requirements 1) and 2) as much as possible, as that in thefirst embodiment. The present embodiment, however, has intermediatenodes each having three child nodes, so that the specific processing ofthe hierarchical compliance operation determiner 114 is somewhat morecomplicated than that in the first embodiment. If a robot is constructedsuch that the postures of the ground contact portions 10 arecontrollable, then the hierarchical compliance operation determiner 114should correct the desired ground contact portion trajectories of theground contact portions 10 to also satisfy the aforesaid requirement 3)as much as possible.

FIG. 25 is a block diagram showing the functions of the hierarchicalcompliance operation determiner 114 of the second embodiment, and itcorresponds to FIG. 17 in the first embodiment. Referring to FIG. 25,the hierarchical compliance operation determiner 114 of the secondembodiment is equipped with, as its components (functional means), acompensating total floor reaction force moment distributor 114 a,compensating angle determiners 114 b, 114 c and 114 d, a correcteddesired ground contact portion position/posture calculator 114 g, adeformation compensation amount calculator 114 n, and a correcteddesired ground contact portion position/posture with deformationcompensation calculator 114 h, as with the first embodiment.Incidentally, in the second embodiment, compensating angles include a145236th compensating angle θ145236, a 145th compensating angle θ145,and a 236th compensating angle θ236 related to the 145236th node, the145th node, and the 236th node, respectively, and the compensating angledeterminers 114 b, 114 c and 114 d determines θ145236, θ145, and θ236.

The following will explain the processing of the hierarchical complianceoperation determiner 114 in the second embodiment, focusing mainly onthe aspects different from the first embodiment.

The hierarchical compliance operation determiner 114 in the secondembodiment, as that in the first embodiment, determines thetranslational force component and the moment component of the desirednode floor reaction force of each node primarily on the basis of anoutput of the desired floor reaction force distributor 102, and alsodetermines the translational force component and the moment component ofthe actual node floor reaction force of each node on the basis of anoutput of the actual floor reaction force detector 108.

More specifically, referring to FIG. 26, a translational force componentFn_ref(n=1, 2, 3,4, 5, 6) of the desired node floor reaction force ofeach leaf node is identical to the translational force component of thedesired node floor reaction force determined by the desired floorreaction force distributor 102, and a translational force componentF145236ref of the desired node floor reaction force of the root node isidentical to a translational force component Ftotalref of the desiredtotal floor reaction force determined by the gait generating device 100.Further, a translational force component Fn_ref(n=145, 236) of a desirednode floor reaction force of an intermediate node other than leaf nodesand the root node is determined to be the translational force componentof the resultant force of the desired node floor reaction forces of thechild nodes of the intermediate node (F145ref=F1ref+F4ref+F5ref,F236ref=F2ref+F3ref+F6ref). In this case, the parent node of theintermediate node is the root node, so that F145ref+F236ref=Ftotalref.Incidentally, FIG. 26 illustrates the translational force componentFn_ref of the desired node floor reaction force of each node in a statewherein all the ground contact portions 10 of the robot 1 in the secondembodiment are in contact with the ground.

The moment component of the desired node floor reaction force of node(more precisely, the moment component of the desired node floor reactionforce having the desired node floor reaction force central point of thenode as its point of action) is always set to zero.

Referring to FIG. 30, a translational force component Fn_act(n=1, 2, 3,4, 5, 6) of the actual node floor reaction force of each leaf node isidentical to the translational force component of the actual floorreaction force of each ground contact portion 10 detected by the actualfloor reaction force detector 108. Further, a translational forcecomponent Fn_act(n=145, 236, 145236) of the actual node floor reactionforce of each node having child nodes is determined to be thetranslational force component of the resultant force of the actual nodefloor reaction forces of the child nodes of the node. Therefore,F145act=F1act+F4act+F5act, F236act=F2act+F3act+F6act, F145236act(=Ftotalact) =F145act+F236act. FIG. 30 illustrates the translationalforce component Fn_act of the actual node floor reaction force of eachnode in the state wherein all the ground contact portions 10 of therobot 1 in the second embodiment are in contact with the ground.

Further, referring to FIG. 31, a moment component of the actual nodefloor reaction force of each leaf node (a moment component having thedesired node floor reaction force central point of the leaf node as itspoint of action) Mn_act(n=1, 2, 3, 4, 5, 6) is basically identical tothe moment component of the actual floor reaction force of each groundcontact portion 10 detected by the actual floor reaction force detector108. In the present embodiment, however, no actual moment occurs at thecentral point of each ground contact portion 10, i.e., the desired floorreaction force central point of the ground contact portion 10;therefore, the moment component of the actual node floor reaction forceof each leaf node is set to zero. Further, regarding the momentcomponent Mn_act of the actual node floor reaction force of each n-thnode (n=145, 236, 145236) that has child nodes, the resultant force ofthe actual node floor reaction forces of the child nodes of the n-thnode is basically determined to be the moment component (this generallydoes not become zero) acting on the desired node floor reaction forcecentral point of the n-th node.

The compensating total floor reaction force moment distributor 114 a inthe second embodiment distributes the aforesaid compensating total floorreaction force moment Mdmd (Mdmdx, Mdmdy) to a 145236th nodecompensating floor reaction force moment M145236dmd, a 145th nodecompensating floor reaction force moment M145dmd, and a 236th nodecompensating floor reaction force moment M236dmd.

The 145236th node compensating floor reaction force moment M145236dmd isthe desired value of the moment to be generated about a desired totalfloor reaction force central point P (desired ZMP) by the translationalforce component of the floor reaction force of each ground contactportion 10 generated by manipulating the 145236th compensating angleθ145236 (by rotating the set of the first, the fourth, and the fifthground contact portions and the set of the second, the third, and thefifth ground contact portions about a desired total floor reaction forcecentral point P(=Q145236) by θ145236).

The 145th node compensating floor reaction force moment M145dmd is thedesired value of the moment to be generated about a desired 145th nodefloor reaction force central point by the translational force componentof the floor reaction force of each ground contact portion 10(specifically, the first, the fourth, and the fifth ground contactportions) generated by manipulating the 145th compensating angle θ145(by rotating the first, the fourth, and the fifth ground contactportions 10 belonging to the 145th node about a desired 145th node floorreaction force central point Q145 by θ145).

The 236th node compensating floor reaction force moment M236dmd is thedesired value of the moment to be generated about a desired 236th nodefloor reaction force central point by the translational force componentof the floor reaction force of each ground contact portion 10(specifically, the second, the third, and the sixth ground contactportions) generated by manipulating the 236th compensating angle θ236(by rotating the second, the third, and the sixth ground contactportions 10 belonging to the 236th node about a desired 236th node floorreaction force central point Q236 by θ236).

In the present embodiment, as with the first embodiment, the posture ofthe robot 1 about the vertical axis is not controlled (because thecomponent of the compensating total floor reaction force moment Mdmdabout the vertical axis is zero), so that M145236dmd, M145dmd, andM236dmd are all moments (horizontal vectors) whose components about thevertical axis are zero, and the compensating angles θ145236, θ145, andθ236 are rotational angles about the horizontal axis. In particular,M145236dmd is the moment about the horizontal axis perpendicular to asegment Q145Q236.

These node compensating floor reaction force moments M145236dmd,M145dmd, and M236dmd are basically determined such that the followingconditions 11) and 12) are satisfied.

11) On the 145th node, the 236th node, and the 145236th node, as definedin the aforesaid first embodiment, when corrected desired node floorreaction force central points Q145mdfd, Q236mdfd, and Pmdfd(=Q145236mdfd) that satisfy the relational expressions of the followingexpressions 7 a to 9 a are introduced, these Q145mdfd, Q236mdfd, andPmdfd exist in their respective predetermined existence permissiblerange.M145236dmd=(Pmdfd−P)*Ftotalref  Expression 7aM145dmd=(Q145mdfd−Q145)*F145ref  Expression 8aM236dmd=(Q236mdfd−Q236)*F236ref  Expression 9a

The existence permissible ranges of Q145mdfd, Q236mdfd, and Pmdfd areset, for example, as shown in FIG. 29(a) in a state wherein all theground contact portions 10 of the robot 1 of the second embodiment arein contact with the ground. More specifically, the existence permissiblerange of Q145mdfd is the region on the triangle in the bold line in thefigure (the sides of and the region in the triangle), and this is theregion set in the triangle having, as its apexes, the desired node floorreaction force central points Q1, Q4, and Q5 of the child nodes of the145th node such that it is not excessively close to the boundary of thetriangle Q1Q4Q5. The existence permissible range of Q236mdfd is similarto the above. Further, the existence permissible range of Pmdfd is theregion on the segment in bold line in the figure, and this is the regionset on a segment Q145Q236 connecting the desired floor reaction forcecentral points Q145 and Q236 of the child nodes of the 145236th node(root node) such that it is not excessively close to the end points ofthe segment Q145Q236.

-   12) Mdmd=M145236dmd+M145dmd+M236dmd is substantially satisfied.

Further, in the present embodiment, the root node (the 145236th node)has two child nodes, so that M145236dmd is limited to a vector in thesame direction as that of a horizontal unit vector (this being denotedby V145236) orthogonal to the segment Q145Q236, as with each nodecompensating floor reaction force moment in the aforesaid firstembodiment. Hence, in the present embodiment, M145236dmd, M145dmd, andM236dmd are determined such that they satisfy the following condition13).

-   13) The component of M145236dmd+M145dmd+M236dmd in the direction of    a vector V145236 takes a value close to the component of Mdmd in the    direction of the vector V145236 as much as possible.

M145236dmd, M145dmd, and M236dmd satisfying these conditions 11) to 13)are determined, for example, as follows. First, M145236dmd is determinedto be the component of Mdmd in the V145236 direction. However, if thecorrected desired node floor reaction force central point Pmdfddetermined by the aforesaid 7 a does not fall within its existencepermissible range, then M145236dmd is corrected such that Pmdfd becomesa point of a boundary of the existence permissible range. Subsequently,Mdmd145 and Mdmd236 are determined such that the vector obtained bysubtracting M145236dmd determined as described above from Mdmdsubstantially agrees with the sum of Mdmd145 and Mdmd236, and thecomponent of Mdmd145+Mdmd23 in the V145236 direction is close to theV145236 component of Mdmd−Mdmd145236 as much as possible and alsosatisfies the aforesaid condition 11). In this case, Mdmd145 and Mdmd236are vectors that are parallel to each other.

Examples of M145236dmd, M145dmd, and M236dmd determined as describedabove are shown in FIG. 29( b). Supplementally, M145236dmd is thehorizontal vector perpendicular to the segment Q145Q236 as describedabove.

If the posture of the robot 1 about the vertical axis is alsocontrolled, the vertical components of M145236dmd, M145dmd, and M236dmdmay be also determined.

Compensating angle determiners 114 b to 114 d in the second embodimentbasically determine node compensating angles θ145236, θ145, and θ236 onthe basis of the difference between the node compensating floor reactionforce moment and the actual node floor reaction force moment (the momenthaving a desired node floor reaction force central point as the point ofaction) of each n-th node (n=145236, 145, 236) such that the differenceis approximated to zero. The examples are shown in FIG. 30 and FIG. 31.In this case, in the present embodiment, θ145 is the angle of rotationin the same direction as the difference M145act−M145dmd and about anaxis that passes the desired 145th node floor reaction force centralpoint Q145, as shown in FIG. 30, and θ236 is the angle of rotation inthe same direction as the difference M236act−M236 dmd and about an axisthat passes the desired 236th node floor reaction force central pointQ236, as shown in FIG. 30. Further, θ145236 is the angle of rotation inthe same direction as the difference M145236act−M145236dmd (thehorizontal direction perpendicular to the segment Q145Q236) and about anaxis that passes the desired total floor reaction force central point P,as shown in FIG. 31.

However, in the present embodiment also, as with the first embodiment,the node compensating angles θ145236, θ145, and θ236 are determined onthe basis of the difference between the node compensating floor reactionforce moment of an n-th node (n=145236, 145, 236) that has been passedthrough a filter and the actual node floor reaction force moment thathas been passed through a filter.

FIG. 32 is a block diagram showing the functions of the compensatingangle determiner 114 b (θ145236 determiner) that determines the nodecompensating angle θ145236 as described above, and FIG. 33 is a blockdiagram showing the functions of the compensating angle determiner 114 c(θ145 determiner) that determines the node compensating angle θ145 asdescribed above. The processing of the compensating angle determiner 114d (θ236 determiner) is the same as the processing of the θ145 determiner114 b, so that it will neither be explained nor shown.

The compensating angle determiner (θ145236 determiner) 114 b firstdetermines the moment M145236act generated about the desired total floorreaction force central point P (the desired 145236th node floor reactionforce central point) according to the aforesaid expression 16 or 17 ifthe translational force component (F1act+F4act+F5act) of the resultantforce of actual first, fourth, and fifth node floor reaction forces actson the desired 145th node floor reaction force central point Q145 andthe translational force component (F2act+F3act+F6act) of the resultantforce of actual second, third, and sixth node floor reaction forces actson the desired 236th node floor reaction force central point Q236. Then,a component M145236actv of the determined M145236act in the direction ofa vector V145236 is determined by scalar product calculation. Here, thevector V145236 is a horizontal unit vector that is perpendicular to thesegment Q145Q236. Regarding the 145236th node, it is unnecessary todetermine the component of M145236act in a vector U145236 directionbecause the positions of the desired floor reaction force central pointsof the child nodes (the 145th node and the 236th node) cannot bemanipulated about the axis in the direction of a horizontal unit vectorU145236 perpendicular to V145236.

Subsequently, as with the first embodiment, a difference M145236errv(=M145236actvfilt−M145236dmdvfilt) between M145236actvfilt obtained bypassing the M145236actv through a low-pass filter and M145236dmdvfiltobtained by passing a component M145236 dmdv of the aforesaid 145236thnode compensating floor reaction force moment M145236dmd in thedirection of a vector V through a compensation filter is multiplied by apredetermined gain K145236 so as to determine the 145236th nodecompensating angle θ145236.

The compensating angle determiner (θ145 determiner) 114 c firstcalculates a moment M145act generated about the 145th node floorreaction force central point Q145 when translational force componentsF1act, F4act, and F5act of actual first, fourth, and fifth node floorreaction forces act on desired first, fourth, and fifth node floorreaction force central points Q1, Q4, and Q5, respectively. In thiscase, M145act to be calculated is formed of components in the directionsof vectors V145 and U145, respectively, which are horizontal unitvectors that are orthogonal to each other. The directions of the vectorsV145 or U145 may be arbitrary.

Subsequently, a difference M145err (=M145actfilt-M145 dmdfilt) betweenM145actfilt obtained by passing the M145act through a low-pass filterand M145dmdfilt obtained by passing the aforesaid 145th nodecompensating floor reaction force moment M145dmd (a component in thevector V direction and a component in the vector U direction) through acompensation filter is multiplied by a predetermined gain matrix K145(diagonal matrix) so as to determine the 145th node compensating angleθ145. θ145 is constructed of an angle component about the axis of thevector V and an angle component about the axis of the vector U.

The processing for determining the 236th node compensating angle θ236 bythe compensating angle determiner (θ236 determiner) 114 d is carried outin the same manner as the processing of the aforesaid θ145 determiner114 c.

The processing by the compensating angle determiners 114 b to 114 dexplained above determines a set of the node compensating anglesθ145236, θ145, and θ236 such that the horizontal component of the actualmoment Mact acting on the desired total floor reaction force centralpoint P approximates the compensating total floor reaction force momentMdmd.

As supplementally explained in conjunction with the first embodiment,the node compensating angles θ145236, θ145, and θ236 may be determinedas follows. For each n-th node (n=145, 236, 145236) having child nodes,a floor reaction force central point that causes the horizontalcomponent of the moment of the actual node floor reaction force of then-th node (the resultant force of the actual node floor reaction forcesof all child nodes of the n-th node) to be zero is determined as anactual n-th node floor reaction force central point. Alternatively, afloor reaction force central point that causes the horizontal componentof the moment, which is obtained by subtracting the moment acting on adesired node floor reaction force central point of each node that isgenerated by the actual node floor reaction force of the node from themoment of the actual node floor reaction force of an n-th node (n=145,236, 145236), to be zero is determined as an actual n-th node floorreaction force central point. Then, the difference between the desirednode floor reaction force central point of the n-th node (n=145, 236,145236) and the actual n-th node floor reaction force central pointdetermined as described above, or the difference between the valuesobtained by passing the node floor reaction force central points throughfilters, respectively, is defined as a control amount, and the nodecompensating angles θ145236, θ145, and θ236 are determined on the basisof the control amount (e.g., θ145236, θ145, and θ236 are determined bymultiplying the control amount by a certain gain) so as to approximatethe control amount to zero.

A corrected desired ground contact portion position/posture calculator114 g in the second embodiment shown in FIG. 25 corrects the desiredground contact portion position/posture (actually the desired groundcontact portion position in the robot shown in FIG. 1) of each groundcontact portion 10 so as to obtain a corrected desired ground contactportion position/posture. More specifically, referring to FIG. 30 andFIG. 31, the desired floor reaction force central points Q1, Q4, and Q5of the first, the fourth, and the fifth nodes, respectively, which arethe child nodes of the 145th node, are rotationally moved by the 145thnode compensating angle θ145 (horizontal vector), the desired floorreaction force central point Q145 of the 145th node being the center ofrotation. The Q1, Q4, and Q5 after the rotational movement are denotedby Q1′, Q4′, and Q5′, respectively. Thus, the 145th node compensatingangle θ145 is the manipulated variable for moving the relativerelationship among the positions of the desired floor reaction forcecentral points Q1, Q4, and Q5 of the first, the fourth, and the fifthnodes, which are the child nodes of the 145th node, without moving theposition of the desired floor reaction force central point Q145 of the145th node.

Similarly, the desired floor reaction force central points Q2, Q3, andQ6 of the second, the third, and the sixth nodes, respectively, whichare the child nodes of the 236th node, are rotationally moved by the236th node compensating angle θ236 (horizontal vector), about thedesired floor reaction force central point Q236 of the 236th node beingthe center of rotation. The Q2, Q3, and Q6 after the rotational movementare denoted by Q2′, Q3′, and Q6′, respectively. Thus, the 236th nodecompensating angle θ236 is the manipulated variable for moving therelative relationship among the positions of the desired floor reactionforce central points Q2, Q3, and Q6 of the second, the third, and thesixth nodes, which are the child nodes of the 236th node, without movingthe position of the desired floor reaction force central point Q236 ofthe 236th node.

These rotational movements are visually shown in FIG. 30.

Subsequently, the desired floor reaction force central points Q145 andQ236 of the 145th and the 236th nodes, which are the child nodes of the145236th node, are rotationally moved by the 145236th node compensatingangle θ145236 about the axial center in the same direction (horizontaldirection orthogonal to a segment Q145Q236) as that of the vector(horizontal vector) of the aforesaid error M145236errv, the desiredfloor reaction force central point P (desired total floor reaction forcecentral point) of the 145236th node being the center of rotation. TheQ145 and Q236 after the rotational movement are denoted by Q145′ andQ236′, respectively, as shown in FIG. 31. Thus, the 145236th nodecompensating angle θ145236 is the manipulated variable for moving therelative relationship between the positions of the desired floorreaction force central points Q145 and Q236 of the 145th and the 236thnodes, which are the child nodes of the 145236th node, without movingthe position of the desired floor reaction force central point P of the145236th node (root node).

Next, referring to FIG. 31, the desired node floor reaction forcecentral points Q1′, Q4′, and Q5′ after the previous rotational movementare moved in parallel by a vector Q145_Q145′. This provides finalcorrected desired node floor reaction force central points Q1″, Q4″, andQ5″ of the first, the fourth, and the fifth nodes. Similarly, thedesired node floor reaction force central points Q2′, Q3′, and Q6′ afterthe previous rotational movement are moved in parallel by a vectorQ236_Q236′. This provides final corrected desired node floor reactionforce central points Q2″, Q3″, and Q6″ of the second, the third, and thesixth nodes.

Lastly, the desired ground contact portion position of an n-th groundcontact portion (n=1, 2, 3, 4, 5, 6) is moved in parallel (substantiallyin the vertical direction) by a vector Qn_Qn′. This corrects the desiredground contact portion position of each ground contact portion 10 (moreprecisely, the relative relationship among the desired ground contactportion positions of the ground contact portions 10). More specifically,for each node having child nodes, the manipulated variable (correctionamount) of the relative relationship (relative positional relationship)among the desired ground contact portion positions of the ground contactportions 10, which are the descendant nodes of the node, is determinedon the basis of the compensating angles θ145, θ236, and θ145236, andcombining the manipulated variables (correction amounts) corrects themutual relative relationship among the desired ground contact portionpositions of the ground contact portions 10.

In a robot in which the postures of ground contact portions arecontrollable and floor reaction force moments can be generated about thedesired floor reaction force central points of the ground contactportions, the operation of a foot posture rotation about a desiredground contact portion floor reaction force central point (desired nodefloor reaction force central point) of each ground contact portion maybe performed by the technique shown in Japanese Unexamined PatentApplication Publication No. H10-277969 (composite-compliance control).More specifically, as supplementally explained in conjunction with thefirst embodiment, the desired posture of the n-th ground contact portionmay be corrected about Q″ after the n-th ground contact portion is movedin parallel as described above.

A deformation compensation amount calculator 114 n in the secondembodiment shown in FIG. 25 determines, as with that in the aforesaidfirst embodiment, a deformation compensation amount En_cmpn(n=1, 2, 3,4, 5, 6) for compensating for an influence caused by the deformation ofthe compliance mechanism 42 of each of the legs #1 to #6. Morespecifically, the deformation compensation amount calculator 114 ndetermines a deformation amount En_mdfd(n=1, 2, 3, 4, 5, 6) of thecompliance mechanism 42 or the like of the legs #1 to #6 expected tooccur due to a corrected desired ground contact portion floor reactionforce Fn_refmdfd(n=1, 2, 3, 4, 5, 6) of each ground contact portion 10(the desired ground contact portion floor reaction force corrected byadding the node compensating floor reaction force moments M145236dmd,M145dmd, and M236dmd to a desired ground contact portion floor reactionforce) output from the compensating total floor reaction force momentdistributor 114 a in the second embodiment by using a mechanismcompliance model. This En_mdfd is multiplied by (−1) to determine thedeformation compensation amount En_cmpn.

The corrected desired ground contact portion position/posture withdeformation compensation calculator 114 h in the second embodiment shownin FIG. 25 determines, as with that in the first embodiment, thecorrected desired ground contact portion position/posture withdeformation compensation by adding a corresponding deformationcompensation amount En_cmpn to the corrected desired ground contactportion position/posture (the position/posture determined by theaforesaid corrected desired ground contact portion position/posturecalculator 114 g) of each ground contact portion 10.

The above is the detailed explanation of the processing of thehierarchical compliance operation determiner 114 in the presentembodiment (the second embodiment).

The arithmetic processing of the control device 50 other than thatexplained above is the same as that of the first embodiment.

Incidentally, the hierarchical structure in the second embodiment may bealtered according to an operation mode (traveling mode) or the like ofthe robot 1, as in the case of the first embodiment. For example, thehierarchical structure may be set as shown in FIG. 34. The example inthe figure is equipped with, as intermediate nodes, a 12th node having afirst ground contact portion and a second ground contact portion aschild nodes (leaf nodes), a 34th node having a third ground contactportion and a fourth ground contact portion as child nodes (leaf nodes),a 56th node having a fifth ground contact portion and a sixth groundcontact portion as child nodes (leaf nodes), and a 3456th node having a56th node and a 34th node as child nodes, the root node having the3456th node and the 12th node as child nodes. This arrangement makes itpossible to perform compliance operations and floor configurationestimations, which will be discussed hereinafter, more accurately insome cases. Incidentally, the meanings of the reference characters inthe figure are the same as those shown in the aforesaid FIG. 3( b) orFIG. 24 or the like.

THIRD EMBODIMENT

An explanation will now be given of a third embodiment in which a floorconfiguration estimating function and a function for correcting theoperation of the robot 1 on the basis of the result of the estimationhave been added to the robot 1 (six-legged robot) of the secondembodiment. In the present embodiment, the explanation will be focusedmainly on the six-legged robot shown in the second embodiment; however,supplemental explanation will be added also to a four-legged robot insome cases. In addition, for the convenience of the understanding of thepresent embodiment, a two-legged robot will be referred to in somecases.

The mechanical construction of the robot 1 in the present embodiment isthe same as that shown in FIG. 1 explained in the first or the secondembodiment (except that the robot 1 has six legs #1 to #6). Hence, theexplanation of the mechanical construction of the robot 1 will beomitted. The functional construction of a control device 50 provided onthe robot 1 in the present embodiment is also the same as that shown inthe aforesaid FIG. 2. In the present embodiment, however, thehierarchical compliance operation determiner 114 in FIG. 2 has newlyadded functions, making it different from that in the second embodiment.Further, the processing of the components of the control device 50except for the hierarchical compliance operation determiner 114 is thesame as that of the second embodiment. Thus, the explanation of thepresent embodiment will be focused mainly on the processing of thehierarchical compliance operation determiner 114, and a detailedexplanation of the processing of the control device 50 other than thatwill be omitted.

FIG. 35 is a block diagram showing the processing functions of thehierarchical compliance operation determiner 114 in the presentembodiment. Of the functions, those functions that are different fromthose in the second embodiment will be explained. A floor configurationestimator 130 as a floor configuration estimating means and an adder 132that adds an estimated floor configuration error (more specifically, theestimated value of floor height error, which will be discussed later,related to each ground contact portion 10) output (estimated) by thefloor configuration estimator 130 to corrected desired ground contactportion position/posture have been newly added to the hierarchicalcompliance operation determiner 114 in the present embodiment, andoutputs of the adder 132 in place of corrected desired ground contactportion position/posture are supplied to the corrected desired groundcontact portion position/posture with deformation compensationcalculator 114 h. The rest of the processing of the components of thehierarchical compliance operation determiner 114 is the same as theprocessing thereof in the second embodiment.

Accordingly, the control processing of the main routine of the controldevice 50 in the present embodiment is partly different from the controlprocessing shown in the flowchart of the aforesaid FIG. 9. FIG. 36 is aflowchart showing the control processing of the main routine of thecontrol device 50 in the present embodiment. As illustrated, in thepresent embodiment, the processing for estimating a floor configurationerror (the processing of the floor configuration estimator 130) in S37is newly added after S36. Furthermore, in S38′, desired ground contactportion position/posture are corrected on the basis of the compensatingangles θ145236, θ145, and θ236 explained in the aforesaid secondembodiment and the floor configuration error estimated in S37, and thedesired ground contact portion position/posture after the correction isfurther corrected on the basis of a deformation compensation amount soas to obtain corrected desired ground contact portion position/posturewith deformation compensation, which are final desired ground contactportion position/posture. In this case, more specifically, correcteddesired ground contact portion position/posture are determined on thebasis of the compensating angles θ145236, θ145, and θ236, as in thesecond embodiment, then the corrected desired ground contact portionposition/posture are corrected on the basis of a floor configurationerror, and this is further corrected on the basis of a deformationcompensation amount, thereby obtaining corrected desired ground contactportion position/posture with deformation compensation. The items otherthan the above are the same as the processing of FIG. 9.

Hereinafter, the aspects of the present embodiment that are differentfrom the second embodiment will be specifically explained.

Before starting the detailed explanation of the present embodiment, theconcepts and terms to be used when the floor configuration estimator 130performs estimation will be defined as follows. In the explanation here,for the sake of convenience, schematic diagrams of an average robot,rather than limiting to the six-legged robot 1 in the presentembodiment, will be used.

As shown in FIG. 37, FIG. 38, and FIG. 39, a floor (or a floor surface)supposed in a desired gait is referred to as “supposed floor.” An actualfloor on which a robot travels is referred to as “actual floor.” For theconvenience of explanation, FIG. 37 shows the four-legged robotexplained in the first embodiment, and FIG. 38 and FIG. 39 show atwo-legged robot. The meanings of the terms explained below remain thesame in any multi-legged robots, including the six-legged robot 1 in thepresent embodiment.

The desired n-th ground contact portion floor reaction force centralpoint Qn defined in the hierarchical compliance control explained in theaforesaid first and the second embodiments has been the point set at thecentral point of an n-th ground contact portion; however, the floorreaction force central point Qn may alternatively be set on the groundcontact surface (bottom surface) of the n-th ground contact portion. Inthis case, in the desired gait, the point on the supposed floor surfacethat is supposed to be in contact with the desired n-th ground contactportion floor reaction force central point Qn is referred to as“supposed n-th floor contact point Dn.”

As is obvious from the definition, during the period in which the n-thground contact portion is in contact with the ground in the desired gaitof a robot, the desired n-th ground contact portion floor reaction forcecentral point Qn and the supposed n-th floor contact point Dn share thesame coordinates as observed from a supporting leg coordinate system(global coordinate system). In comparison to this, the point at whichthe point that corresponds to the desired n-th ground contact portionfloor reaction force central point Qn on the bottom surface of an actualn-th ground contact portion when the robot 1 is actually traveling comesin contact with an actual floor is referred to as “actual n-th floorcontact point Dnact.”

Examples showing the relationships among these points are shown in FIG.37, FIG. 38, and FIG. 39. Incidentally, FIG. 37 shows a four-leggedrobot viewed from the direction of the normal line of a vertical planethat passes a desired first ground contact portion floor reaction forcecentral point Q1 and a desired second ground contact portion floorreaction force central point Q2 (that is, substantially sideways), FIG.38 shows a traveling (walking) two-legged robot viewed from thedirection of the normal line of a vertical plane that passes the desiredfirst ground contact portion floor reaction force central point Q1 andthe desired second ground contact portion floor reaction force centralpoint Q2 (that is, substantially sideways), and FIG. 39 shows atwo-legged robot substantially in an upright posture viewed from thedirection of the normal line of a vertical plane that passes the desiredfirst ground contact portion floor reaction force central point Q1 andthe desired second ground contact portion floor reaction force centralpoint Q2 (that is, substantially from rear).

In these FIG. 37 to FIG. 39, the sections of the supposed floors on theaforesaid vertical planes are indicated by thin lines, while thesections of actual floors in the aforesaid vertical planes are indicatedby thick lines. Incidentally, in FIG. 37, the desired posture of therobot (the entire posture of the robot at an instantaneous value of adesired gait) and the actual posture are indicated by dashed lines andsolid lines, respectively. Further, in FIG. 38 and FIG. 39, the desiredpostures of the robots (the entire postures of the robots atinstantaneous values of desired gaits) and actual ground contact portionposition/posture are indicated by thin lines and thick lines,respectively. Actual n-th floor contact points in these situations arethe points on actual floor surfaces, and they are at the positions shownin FIG. 37, FIG. 38, and FIG. 39.

The configuration difference of an actual floor surface with respect toa supposed floor surface is referred to as a floor configuration error.As the indexes for quantitatively representing floor configurationerrors, an n-th node floor height error and an n-th node floorinclination error are defined as follows.

The height of a floor surface at an n-th floor contact point is referredto as “n-th ground contact portion floor height.” In relation to an n-thnode that is a leaf, the difference between an actual n-th groundcontact portion floor height and a supposed n-th ground contact portionfloor height is referred to as “n-th ground contact portion floor heighterror” or “n-th node floor height error.” The inclination of the floorsurface at an n-th floor contact point is referred to as “n-th groundcontact portion floor inclination.” In relation to an n-th node that isa leaf, the difference between an actual n-th ground contact portionfloor inclination and a supposed n-th ground contact portion floorinclination is referred to as “n-th ground contact portion floorinclination error” or “n-th node floor inclination error.” An example ofthe ground contact portion floor inclination error is shown in FIG. 39.

With respect to all j-th nodes, which are leaf nodes, the set of nodecompensating angles for the relationship between desired j-th groundcontact portion position/posture and the height and inclination of asupposed j-th floor surface (specifically, the height and inclination ofa supposed floor surface at a supposed j-th floor contact point) and therelationship between corrected desired j-th ground contact portionposition/posture that has been corrected by a compliance operation usinga set of node compensating angles and the height and inclination of anactual j-th floor surface (specifically, the height and inclination ofan actual floor surface at an actual j-th floor contact point) to agreewith each other is referred to as “(set of) node floor inclinationerrors),” and among these, the component of the node floor inclinationerror corresponding to an n-th node compensating angle is referred to as“n-th node floor inclination error.” If an n-th node is a leaf node,then the “n-th node floor inclination error” thus defined agrees with“the n-th node floor inclination error” (=the difference between anactual n-th ground contact portion floor inclination and a supposed n-thground contact portion floor inclination) defined in relation to leafnodes previously described.

After all, the set of node floor inclination errors corresponds to acompensation amount necessary to shift all ground contact portions of arobot that is traveling according to a desired gait from a state inwhich they are parallel to a desired floor surface to a state in whichthey are parallel to an actual floor surface.

Therefore, by estimating a floor configuration error while walking andby adding the estimated floor configuration error to desired groundcontact portion position/posture, the actual floor reaction force momentof each node will be the same as that when the robot is walking on asupposed floor even if there is a floor configuration error. Naturally,the actual floor reaction force moment of the ground contact portion,which is the actual floor reaction force moment of a leaf node, agreeswith a desired ground contact portion floor reaction force moment.

Normally, a floor configuration may be represented using the “n-th nodefloor inclination error” defined as above. If, however, a certain nodehas three child nodes, and the desired floor reaction force centralpoints of the three child nodes are aligned on the same straight line orif a certain node has four or more child nodes, then it would bedifficult to represent a floor configuration.

Hence, in the following explanation, the following expressions that canbe generally applied will be used for n-th nodes that are not leaves.The weights used to determine a predetermined weighted average in thefollowing definition are to be the same as the aforesaid weightsdetermined as described above by the desired floor reaction forcedistributor 102.

Definition:

-   When all the heights and inclinations of a desired ground contact    portion are set to agree with the corresponding heights and    inclinations of an actual floor surface, and when the desired floor    reaction force central point of an arbitrary node is expressed by a    predetermined weighted average of the desired floor reaction force    central points of all child nodes thereof (i.e., an internally    dividing point based on a predetermined internal division ratio), on    an arbitrary n-th node, the height obtained by subtracting the    height (the position in the vertical direction) of the desired floor    reaction force central point of a parent node of the n-th node from    the height (the position in the vertical direction) of the desired    floor reaction force central point of the n-th node is referred to    as “actual n-th node relative floor height.”    Definition:    When all the heights and inclinations of a desired ground contact    portion are set to agree with the corresponding heights and    inclinations of a supposed floor surface, and when the desired floor    reaction force central point of an arbitrary node is expressed by a    predetermined weighted average of the desired floor reaction force    central points of all child nodes thereof (i.e., an internally    dividing point based on a predetermined internal division ratio),    regarding an arbitrary n-th node, the height obtained by subtracting    the height (the position in the vertical direction) of the desired    floor reaction force central point of a parent node of the n-th node    from the height (the position in the vertical direction) of the    desired floor reaction force central point of the n-th node is    referred to as “supposed n-th node relative floor height.”

The height obtained by subtracting a supposed n-th node relative floorheight from an actual n-th node relative floor height is referred to asan “n-th node relative floor height error.” The n-th node relative floorheight error will take the same value when defined as follows.

Definition:

When all the heights and postures of desired ground contact portions aremade to agree with corresponding ground contact portion floor heighterrors and ground contact portion floor inclination errors, and thedesired floor reaction force central point of an arbitrary node isexpressed by a predetermined weighted average of the desired floorreaction force central points of all child nodes thereof (i.e., aninternally dividing point based on a predetermined internal divisionratio), on an arbitrary n-th node, the height obtained by subtractingthe desired floor reaction force central point of a parent node of then-th node from the desired floor reaction force central point height ofthe n-th node is referred to as “n-th node relative floor height error.”

As is obvious from the definition of the n-th node relative floor heighterror described above, regarding each node having child nodes, an n-thnode relative floor height error of all child nodes thereof representsthe relative relationship of floor height errors. Further, regardingeach node having child nodes, the weighted average value of all childnodes thereof is zero.

In the present embodiment (the third embodiment), the difference betweenan actual floor surface and a supposed floor surface on a desired gait(i.e., a floor configuration error) is compensated so as to correct thedesired positions/postures of ground contact portions 10 to make theground contact portions 10 that are to be in contact with the groundproperly come in contact with the actual floor surface. Further, in thepresent embodiment, the n-th node relative floor height error defined asdescribed above is used as a floor configuration parameter representinga floor configuration error, and based on this, each n-th ground contactportion floor height error (floor configuration error) is estimated.Then, based on each estimated n-th ground contact portion floor heighterror (hereinafter referred to simply as the n-th floor height error insome cases), the desired position of the n-th ground contact portion 10is corrected.

Based on the above premises, essential sections of the presentembodiment will be explained in detail.

The inputs into the floor configuration estimator 130 of thehierarchical compliance operation determiner 114 shown in FIG. 35 are,in general, roughly divided as follows.

-   1) Desired floor reaction force central points (desired total floor    reaction force central points P and desired n-th ground contact    portion floor reaction force central points Qn(n=1, 2, 3, 4, 5, 6)).-   2) Supposed floor surface configuration (coordinates of supposed    n-th floor contact points and supposed n-th ground contact portion    floor inclinations).-   3) Final desired postures to be followed by an actual robot    (corrected desired ground contact portion positions/postures with    deformations compensation) or actual joint displacements or    frequency weighted average thereof (weighted average based on a    weight having a frequency characteristic).-   4) Body posture inclination errors.-   5) Actual floor reaction forces (the translational force component    and the moment component of an actual n-th ground contact portion    floor reaction force).

Incidentally, in the robot 1 in the present embodiment, the postures ofthe ground contact portions 10 cannot be controlled, so that thesupposed n-th ground contact portion inclination of a supposed floorsurface configuration is unnecessary.

Further, in the robot 1 in the present embodiment, the moment componentof an actual n-th ground contact portion floor reaction force of anactual floor reaction force is zero. This, therefore, is not required tobe supplied to the floor configuration estimator 130. However, in theexplanation of the present embodiment hereinafter, the n-th groundcontact portion floor reaction force generally includes the momentcomponent of an actual n-th ground contact portion floor reaction forcealso, considering a case where the postures of the ground contactportions 10 are controllable. Further, in the following explanation, thetotal number of ground contact portions will be frequently referred toas “last leaf node number” so as to make it possible to expandably applythe present embodiment easily also to a case where the number of groundcontact portions is other than four or six. For example, in thesix-legged robot 1 shown in FIG. 1, the last leaf node number is “6”.

FIG. 40 is a block diagram showing the processing functions of the floorconfiguration estimator 130. The functional elements constituting thefloor configuration estimator 130 shown in FIG. 40 will be explained.The floor configuration estimator 130 is equipped with a mechanismcompliance model 134.

The mechanism compliance model 134 determines the deformation amount ofa compliance mechanism 42 or the like of each of the legs #1 to #6 (thecompliance mechanism 42 and a link mechanism of each leg) when each ofthe ground contact portions 10 is subjected to an actual floor reactionforce Fn_act(n=1, 2, . . . , last leaf node number), and adds thedetermined deformation amount to the corrected desired ground contactportion position/posture with deformation compensation, which aresupplied to the aforesaid robot geometric model 110 (refer to FIG. 2),so as to determine estimated n-th ground contact portionposition/posture (n=1, 2, . . . , last leaf node number), which are theestimated values of the position/posture of an n-th ground contactportion (each ground contact portion) after mechanism deformation (morespecifically, the estimated values of the n-th ground contact portionposition/posture when it is assumed that an actual body posture agreeswith a desired body posture). Incidentally, the corrected desired groundcontact portion position/posture with deformation compensation used inthis case are the past values of the values or the like determined atthe last control cycle.

When determining the estimated ground contact portion position/postureafter mechanism deformation, if a follow-up delay of control of a jointdisplacement of the robot 1 cannot be ignored, then the position/postureobtained by passing corrected desired ground contact portionposition/posture with deformation compensation through a low-pass filtercorresponding to the follow-up delay may be used instead of using thecorrected desired ground contact portion position/posture withdeformation compensation as it is.

Alternatively, from a detected value of an actual joint displacement ofthe robot 1, an actual ground contact portion position/posture withoutmechanism deformation, which is an actual ground contact portionposition/posture in a case where there is no mechanism deformation, aredetermined through a robot geometric model (the same model as the robotgeometric model 110 in FIG. 2), and the determined values may be used inplace of the corrected desired ground contact portion position/posturewith deformation compensation.

Alternatively, a weighted average of the aforesaid actual ground contactportion position/posture without mechanism deformation and the correcteddesired ground contact portion position/posture with deformationcompensation may be determined by using a frequency weight (weighthaving a frequency characteristic), and this may be used in place of thecorrected desired ground contact portion position/posture withdeformation compensation.

Strictly speaking, the actual ground contact portion position/posturewithout mechanism deformation mentioned above should be used; however,any one of the above may be used without much difference as long as thecapability of joint displacement control is high.

Incidentally, the mechanism compliance model 134 is explained in detailin Japanese Unexamined Patent Application Publication No. H10-277969previously proposed by the present applicant; therefore, no moreexplanation will be given herein.

Supplementally, in a case where an input to the robot geometric model110 in FIG. 2 does not include deformation compensation, that is, ifcorrected desired ground contact portion position/posture withoutdeformation compensation (the position/posture determined by theaforesaid corrected desired ground contact portion position/posturecalculator 114 g) is supplied to the robot geometric model 110, then thecorrected desired ground contact portion position/posture withoutdeformation compensation may be supplied to the mechanism compliancemodel 134 of the floor configuration estimator 130.

Returning to the explanation of FIG. 40, the estimated ground contactportion position/posture after mechanism deformation (n=1, 2, . . . , 6)is then rotationally moved by the aforesaid body posture inclinationerror θberr, about the desired total floor reaction force central pointP being the center of the rotation, thereby to determine n-th estimatedground contact portion position/posture after mechanism deformation(n=1, 2, . . . , last leaf node number) observed from a globalcoordinate system (supporting leg coordinate system). Subsequently, theposition of the point corresponding to the desired n-th ground contactportion floor reaction force central point after the mechanismdeformation observed from the global coordinate system is determined onthe basis of the n-th estimated ground contact portion position/postureafter the mechanism deformation (n=1,2, . . . , last leaf node number)observed from the global coordinate system and the position of thedesired n-th ground contact portion floor reaction force central pointobserved from the desired ground contact portion position of an n-thground contact portion (the desired position of the representative pointof the n-th ground contact portion). Then, the determined position ofthe point is defined as an instantaneous estimated floor contact pointposition (instantaneous estimated n-th floor contact point position)Qn_estm′. This Qn_estm′ corresponds to the estimated value of theinstantaneous position of an actual n-th floor contact point. The methodfor determining the instantaneous estimated floor contact point positionQn_estm′ are equivalent to determining Q″ of expression 21 in JapaneseUnexamined Patent Application Publication No. H10-277969, so that nofurther detailed explanation thereof will be given herein.

Subsequently, the result obtained by subtracting the supposed n-th floorcontact point position Dn from the instantaneous estimated n-th floorcontact point position Qn-estm′ provides a bias-included instantaneousn-th ground contact portion floor height error. This Zfn_with_bias′corresponds to the instantaneous estimated value of an n-th groundcontact portion floor height error. In a case where no actual height ofa body is detected, as with the present embodiment, a bias error isusually included. Hence, Zfn_with_bias′ is referred to as thebias-included instantaneous n-th ground contact portion floor heighterror.

Here, as previously defined, the supposed n-th floor contact point Dnlies at the same position as a desired n-th ground contact portion floorreaction force central point Qn in a period during which an n-th groundcontact portion is in contact with the ground. In a desired gait,immediately before an n-th ground contact portion comes in contact withthe ground, the position of the desired n-th ground contact portionfloor reaction force central point Qn that is expected at the nextground contact is defined as the supposed n-th floor contact point Dn.In the desired gait, immediately after the n-th ground contact portionleaves a floor, the position of the desired n-th ground contact portionfloor reaction force central point Qn that was expected at the time ofleaving the floor is defined as the supposed n-th floor contact pointDn.

Subsequently, an estimated n-th node floor height error Zfn_estm (n=1,2, . . . , last leaf node number) is determined by the floor heighterror estimation processing subroutine of the flowchart of FIG. 41 onthe basis of the bias-included instantaneous n-th ground contact portionfloor height error Zfn_with_bias′ (n=1, 2, . . . , last leaf nodenumber) and the n-th ground contact portion floor reaction force Fn_act(n=1, 2, . . . , last leaf node number).

Here, “hierarchical relativization processing” (or “hierarchicalrelativization”) used in the floor height error estimation processingsubroutine or the like will be explained.

The hierarchical relativization processing is generally defined as theprocessing for determining the output values of all nodes relative tothe sets of input values (the values of predetermined types of stateamounts) to all leaf nodes. More specifically, the hierarchizationrelativization processing is the processing for determining node outputvalues such that the weighted average of output values corresponding toall childe nodes of an arbitrary node that is not a leaf node is zeroand the input value (state amount) of an arbitrary leaf node agrees withthe sum of the output value of the node and the output values of allancestor nodes of the node.

Hereinafter, determining the value of a certain type of output BB by thehierarchical relativization processing from a certain type of input(state amount) AA to a leaf node will be referred to as “determining BBby hierarchically relativizing AA.”

The following will explain the algorithm of the hierarchicalrelativization processing. In general, an input of the hierarchicalrelativization processing is an n-th ground contact portion height Zfn(ndenoting a leaf node number) and an output thereof is an n-th noderelative height Zn_re1(n=1, 2, . . . , last node number). Incidentally,the “last node number” means the largest number among all node numbers,and the last node number is, for example, 145236 in the hierarchicalstructure explained in the second embodiment. Further, in theexplanation of the algorithm, the “n-th ground contact portion height”is a designation that generically represents an input of thehierarchical relativization processing, such as the aforesaid n-thground contact portion floor height or an n-th ground contact portionfloor height error, and the “n-th node relative height” is a designationthat generically represents an output of the hierarchical relativizationprocessing, such as an n-th ground contact portion relative floor heighterror, which will be discussed later.

First, a bias-included n-th node height Zn_with_bias is determined asfollows. If an n-th node is a leaf node (i.e., the ground contactportion), then the value of an n-th ground contact portion height Zfn issubstituted into the bias-included n-th node height Zn_with_bias. If then-th node is not a leaf node (i.e., the ground contact portion), thenthe weighted average of the bias-included n-th node heights Zn_with_biasof all child nodes of the n-th node is determined. The determinedweighted average is defined as the bias-included n-th node heightZn_with_bias. It should be noted that a weight Wj(j=1,2, . . . )determined by the aforesaid desired floor reaction force distributor 102is used as the weight for each child node.

More specifically, the bias-included n-th node height Zn_with_bias isobtained by expression 32 given below.If the n-th node is a leaf node; Zn_with_bias=Zfn If the n-th node isnot a leaf node; Zn_bias=Zfn where Σ denotes the total sum on j, whichis j ∈ {set of child node numbers of the n-th node }  Expression 32

According to the above rules, the bias-included node height Zn_with_bias(n=1, 2, . . . , last node number) is determined on every node.

Lastly, an n-th node relative height Zn_re1 is determined by subtractingthe bias-included height Zh_with_bias of a parent node of the n-th node(this is supposed to be an h-th node) from the bias-included n-th nodeheight Zn_with_bias.

More specifically, the n-th node relative height Zn_re1 is determinedaccording to the following expression 33. It should be noted that theh-th node is the parent node of the n-th node.Zn _(—) re1=Zn_with_bias−Zh_with_bias  Expression 33

According to the above rule, the node relative floor height Zn_re1(ndenoting the number of each node) is determined on every node. It shouldbe noted that a node relative height Zk_re1(k denoting the number of aroot node) with respect to a root node is zero. FIG. 42 shows an exampleof calculation of a node relative height of a four-legged robot shown inthe first embodiment, while FIG. 43 shows an example of calculation of anode relative height of the six-legged robot in the present embodiment.

The bias-included n-th node height Zn_re1_with_bias may alternatively bedetermined by the following method, which provides the same results.

If an n-th node is a leaf node (i.e., the ground contact portion), thenthe value of an n-th ground contact portion height Zfn is substitutedinto the bias-included n-th node height Zn_with_bias. If the n-th nodeis not a leaf node (i.e., the ground contact portion), then the weightedaverage of the bias-included heights of the leaf nodes of alldescendants of the n-th node is determined. The determined weightedaverage is defined as the bias-included n-th node height Zn_with_bias.It should be noted that a weight Wj′ is the product of the weight Wjdetermined for the node j by the aforesaid desired floor reaction forcedistributor 102 and the weight determined by the aforesaid desired floorreaction force distributor 102 for an ancestor node of the node j andall nodes, which are the descendant nodes of the n-th node.

More specifically, the bias-included n-th node height Zn_with_bias isobtained by expression 34 given below.If the n-th node is a leaf node; Zn_with_bias =Zfn If the n-th node isnot a leaf node; Zn_with_bias=Σ(Zj_with_bias*Wj′) where Σ denotes thetotal sum on j, which is j∈ {set of leaf node numbers of the descendantsof a node n }  Expression 34

Supplementally, the weight Wj′ in this case agrees with the valueobtained by dividing the desired floor reaction force of each leaf nodeby a desired j-th node floor reaction force if the desired j-th nodefloor reaction force is not zero.

The above is the hierarchical relativization processing.

The subroutine processing of the estimation processing of a floor heighterror will now be explained below by mainly using the flowchart of FIG.41 showing it. Incidentally, this processing uses an n-th ground contactportion floor height error as an input of the hierarchicalrelativization processing, and an n-th node relative floor height erroras an output. The like symbols as those shown in the aforesaidexpressions 32 to 34 or the like will be used as the reference symbolsof these input and output.

Referring to FIG. 41, first, in S50, the aforesaid bias-includedinstantaneous n-th ground contact portion floor height errorZfn_with_bias′ is hierarchically relativized to determine aninstantaneous n-th node relative floor height error Zn_rel′ (n=1, 2, . .. , last node number).

Subsequently, in S52, the output obtained by hierarchically relativizingan estimated n-th ground contact portion floor height error, which is anestimated value of the n-th ground contact portion floor height errordetermined at the last control cycle of the control device 50(hereinafter referred to as the last estimated n-th ground contactportion floor height error Zfn_estm_p), is determined as the lastestimated n-th node relative floor height error Zn_rel_estm_p(n=1, 2, .. . , last node number) . It should be noted that, as the weight of eachnode used in this hierarchical relativization processing, the weightWn(n=1, 2, . . . last node number) determined by the aforesaid desiredfloor reaction force distributor 102 at the current (present) controlcycle of the control device 50 is used.

Supplementally, in general, if the weight determined in the currentcontrol cycle (hereinafter referred to as the current weight) and theweight determined in the last control cycle (hereinafter referred to asthe last weight) are different, then the value of the last estimatedrelative floor height error of each node determined using the lastweight and the value of the last estimated relative floor height errorof each node determined using the current weight will be inconvenientlydifferent values even if the configuration of an actual floor does notchange. Hence, the last estimated node relative floor height error ofeach node is calculated anew by using the current weight as describedabove instead of directly using the estimated relative floor heighterror of each node determined at the last control cycle as the lastestimated node relative floor height error of each node.

Subsequently, in S54, an n-th node relative floor height errorcorrection amount candidate value Zn_inc_cand(n=1, 2, . . . , last nodenumber) is determined according to expression 35 given below. TheZn_inc_cand means the provisional value of the correction amount of theerror when the n-th node relative floor height error is updated at thecurrent control cycle.

In this case, the n-th node relative floor height error correctionamount candidate value Zn_inc_cand is set to the product of thedifference between the instantaneous n-th node relative floor heighterror Zn_rel′ and the last estimated n-th node relative floor heighterror Zn_rel_estm_p and a predetermined coefficient (ΔT/(Testm+ΔT)), asshown by expression 35 given below.Zn _(—) inc _(—) cand=(Zn _(—) rel′−Zn _(—) rel _(—) estm _(—)p)*ΔT/(Testm+ΔT)  Expression 35

where Testm in expression 35 denotes a time constant (the time constantof a primary delay) at the estimation (update) of an n-th node floorheight error, and ΔT is a control cycle of the control device 50.

Thus, when Zn_inc_cand is set, Zn_inc_cand is added to Zn_rel_estm_p foreach control cycle of the control device 50, causing the value of theresult of the addition to change so as to gradually approximate toZn_rel′.

Subsequently, in S56, a node request mode (an n-th request modemdn_dmd), which is a required value of the mode of each node, isdetermined on the basis of a timing of a desired gait.

Specifically, in the aforesaid six-legged robot 1, an n-th node requestmode (n=1, 2, 3, 4, 5, 6), a 145th node request mode md145dmd, a 236thnode request mode md236dmd, and a 145236th node request mode md145236dmdare set as shown in the timing chart of FIG. 44 with respect to theelapse of time of a desired gait, and a current request mode isdetermined on the basis thereof. However, if the gait is different, thenthe pattern of a node request mode should be changed accordingly.Incidentally, regarding ON/OFF in the timing charts of a first stage(the uppermost stage) and a second stage in FIG. 44, the state in whichthe ground contact portion is in contact with the ground corresponds toON, while the state in which it is not in contact with the groundcorresponds to OFF.

Supplementally, as the present applicant has disclosed in JapaneseUnexamined Patent Application Publication No. H10-277969, in the case ofa robot that has feet as ground contact portions and that is capable ofestimating floor inclination errors at the ground contact portions, therequest mode corresponding to the floor inclination error estimation ofthe ground contact portions should be set as explained in thePublication.

As shown in FIG. 44, the mode of each node (hereinafter referred tosimply as the node mode) comes in ready mode, hold mode, and reset mode.

The ready mode is the mode for estimating a floor configuration. Thehold mode is the mode for holding (for maintaining an immediatelypreceding value) the estimated value of the floor configuration (anestimated n-th ground contact portion floor height error). The hold modeis made to exist in a period during which the estimated value of thefloor configuration may diverge and a period during which the accuracyof the estimated value of the floor configuration may deteriorate.Further, the reset mode is the mode for shifting the estimated value ofthe floor configuration to a predetermined initial value before the nextfloor configuration estimation is started (the next ready mode begins).

Regarding an n-th node (n=1, 2, 3, 4, 5, 6), which is a leaf node, if ann-th ground contact portion is in contact with the ground on a desiredgait (that is, if a desired n-th ground contact portion floor reactionforce is not zero), then the n-th node request mode is set to the readymode. Thereafter, when the n-th ground contact portion leaves a floor onthe desired gait (that is, when the desired n-th ground contact portionfloor reaction force becomes zero), the n-th node request mode is set tothe hold mode immediately thereafter. Then, after a while, the n-th noderequest mode is set to the reset mode. Further, immediately before then-th ground contact portion comes in contact with the ground on thedesired gait, the n-th node request mode is set to the ready mode.

Regarding an n-th node (n=145, 236, 145236), which is not a leaf node,that is, an n-th node having child nodes, if at least one ground contactportion belonging to the n-th node is in contact with the ground on thedesired gait, then the n-th node request mode is set to the ready mode.Thereafter, when all ground contact portions belonging to the n-th nodeleave the floor on the desired gait, then the n-th node request mode isset to hold until immediately thereafter. Alternatively, the n-th noderequest mode may be set to the hold mode from immediately before all theground contact portions belonging to the n-th node leave the floor.Then, after a while, the n-th node request mode is set to the resetmode. Further, immediately before at least one of the ground contactportions belonging to the n-th node comes in contact with the ground onthe desired gait, the n-th node request mode is set to the ready mode.

Subsequently, in S58, it is determined whether an estimation enablecondition is satisfied, and the mode of each node is finally determinedon the basis of the result of the determination and the node requestmode. The mode of each node to be determined is one of the aforesaidready mode, hold mode, and reset mode. The aforesaid node request modehas been determined on the basis of whether the ground contact portionsare in contact with the ground or not on the desired gait. In S58, themode of each node is determined, considering whether the ground contactportions are actually in contact with the ground or not.

The estimation enable condition is satisfying either expression 36 or 37given below. In this case, if neither expression 36 or 37 is satisfied,then the estimation enable condition does not hold.Fn_act_z>Fn_min  Expression 36Zn_rel′<Zn_rel_estm_p  Expression 37

where Fn_act_z denotes the translational force vertical component of anactual n-th ground contact portion floor reaction force (n=1, 2, . . . ,last leaf node number).

The estimation enable condition is a condition that when an n-th noderelative floor height error is estimated (when a floor configurationerror is estimated), the estimated value does not diverge. Thedivergence here means a situation in which, when a correcting operationis performed to modify corrected desired ground contact portionposition/posture so as to offset the influence of an actual n-th floorheight error by using an estimated n-th floor height error Zfn_estm (theestimated value of the floor configuration error) that has beenestimated as it will be discussed later, the estimated n-th floor heighterror Zfn_estm continues to increase and an n-th ground contact portionmoves away from the floor (floats further from the floor).

If the estimation of Zfn_estm is ideally executed, a predeterminedpermissible value Fn_min may be zero, whereas in actuality, a detectionerror of Fn_act takes place, causing an estimated value to diverge insome cases. To prevent it, Fn min is set to be a value that issufficiently larger than a detection error of Fn_act.

In the present embodiment, the estimation enable condition is thecondition in which divergence described above does not occur. However,the estimation enable condition may alternatively be a condition inwhich a predetermined accuracy of estimating a floor configuration canbe secured. Hence, in place of the translational force verticalcomponent of the actual n-th ground contact portion floor reaction forceFn_act, a component that is perpendicular to a supposed floor surface(or an estimated floor surface (estimated actual floor surface)) out ofthe translational force components of the actual n-th ground contactportion floor reaction force Fn_act may be used.

Incidentally, if a floor configuration error (a configuration errorbetween an actual floor surface and a supposed floor surface) is simplyestimated and not reflected in a walking operation, that is, if thecorrection of the corrected desired ground contact portionposition/posture to cancel the influence of a floor configuration erroris not carried out by using an estimated value of the floorconfiguration error, then the estimated value of the floor configurationerror does not diverge.

The mode of each node is determined on the basis of the result of thedetermination on whether the estimation enable condition is satisfiedand a node request mode.

If an n-th node request mode is the ready mode and the estimation enablecondition is satisfied, then the n-th node mode is set to the readymode.

If an n-th node request mode is the ready mode and the estimation enablecondition is not satisfied, then the n-th node mode is set to the holdmode.

If an n-th node request mode is the hold mode, then the n-th node modeis set to the hold mode.

If an n-th node request mode is the reset mode, then the n-th node modeis set to the reset mode.

Thus, an n-th mode is determined to be the ready mode at the end only ifthe request mode is the ready mode and the estimation enable conditionis satisfied.

Subsequently, in S60 to S70, the n-th node relative floor height error(n=1, 2, . . . , last node number) associated with each of all nodes isestimated.

Supplementally, as disclosed in Japanese Unexamined Patent ApplicationPublication No. H10-277969, in the case of a robot that has groundcontact portions (feet) whose postures can be controlled and that iscapable of estimating the floor inclination errors in the ground contactportions, it is preferred to also estimate the floor inclination errorsassociated with the ground contact portions.

Therefore, the processing of S60 to S70 is configured so as to allowfloor configuration errors to be estimated also when the postures ofground contact portions can be controlled.

The following will specifically explain the processing of S60 to S70. InS62, the quantity of child nodes of an n-th node is determined. At thistime, if the number of child nodes (the quantity of child nodes) is two,then in S64, the floor configuration estimation processing for two childnodes, which is the estimation processing of the floor configurationerror for the quantity, is carried out. If the number of child nodes isthree, then in S66, the floor configuration estimation processing forthree child nodes, which is the estimation processing of the floorconfiguration error for the quantity, is carried out. If the number ofchild nodes is zero, then it is determined in S68 whether the n-th nodeground contact portion permits control of a floor reaction force moment.

The processing for the case where the number of child nodes is zero willbe further explained. If the number of child nodes of an n-th node iszero, then the n-th node is a leaf node, and the n-th node correspondsto the ground contact portion. In this case, as described above, first,it is determined in S68 whether the n-th ground contact portion permitscontrol of a floor reaction force moment. Here, for example, in the caseof a robot that has feet whose postures can be controlled as groundcontact portions and its legs do not include free joints, as with aregular bipedal walking robot, floor reaction force moments can begenerated at the ground contact portions. However, in the robot 1(six-legged robot) in the present embodiment, the ground contactportions 10 engage the spherical joints 12, which are free joints;therefore, none of the ground contact portions 10 can generate floorreaction force moments. In this case, no processing is carried out onthe n-th node whose number of child nodes is zero.

Meanwhile, if it is determined in S68 that floor reaction force momentscan be controlled, then ground contact portion floor inclination errorestimation processing is carried out in S70. This processing is theprocessing for estimating the floor inclination error in each groundcontact portion. This processing is the processing in which the footfloor reaction force central point in the floor inclination estimationprocessing in Japanese Unexamined Patent Application Publication No.H10-277969 previously proposed by the present applicant has beenreplaced by the desired floor reaction force central point of an n-thground contact portion in the present embodiment. Hence, furtherdetailed explanation will be omitted in the present description.

Next, the aforesaid floor configuration estimation processing for twochild nodes will be explained with reference to FIG. 45 to FIG. 48. FIG.45 is a flowchart showing the subroutine processing of the floorconfiguration estimation processing for two child nodes, FIG. 46 andFIG. 47 are flowcharts showing the subroutine processing of S6404 andS6408, respectively, of FIG. 45, and FIG. 48 is a flowchart showing thesubroutine processing of S6406 and S6410 of FIG. 45.

In the floor configuration estimation processing for two child nodes,the modes of all child nodes of an n-th node having two child nodes (themode determined in S58 of FIG. 41) are determined in S6400, as shown bythe flowchart of FIG. 45. The results of the determination are dividedinto three, namely, “all ready,” “all reset,” and “others (else).” Inthe following explanation, the two child nodes of the n-th node will bedenoted as the i-th node and the j-th node.

Here, if it is determined that the mode of the child nodes is “allready,” then it is determined in S6402 whether a resultant force Fn_z(=Fi_act_z+Fj_act_z) of the translational force vertical componentsFi_act_z, Fj_act_z of the actual node floor reaction forces of the childnodes of the n-th node is larger than a predetermined value Fn_min2. Inother words, Fn_z denotes the translational force vertical component ofthe resultant force of the actual floor reaction forces of all groundcontact portions belonging to the n-th node.

If the determination result of S6402 is YES, then intra-group completeestimation processing for a node having two child nodes (the processingfor virtually estimating the node relative floor height error of each ofthe two child nodes owned by the n-th node) is carried out in S6404. Inthis processing, estimated node relative floor height errors Zi_rel_estmand Zj_rel_estm of the i-th node and the j-th node, respectively, whichare the two child nodes of the n-th node, are determined (updated)according to the expressions shown in the flowchart of FIG. 46. Morespecifically, Zi_rel_estm will be representatively explained. An i-thnode relative floor height error correction amount candidate valueZi_inc_cand determined in the aforesaid S54 is added to a valueZi_rel_estm p of Zi_rel_estm in the last control cycle thereby todetermine a new estimated i-th node relative floor height errorZi_rel_estm. The same applies to the j-th node.

Further, if the resultant force Fn_z of the floor reaction forces of thetwo child nodes of the n-th node is smaller than the predetermined valueFn_min2 (if the determination result of S6402 is NO), then it means thatthe accuracy of estimating a floor configuration error would beexcessively deteriorated, so that no substantial estimation processingis carried out, and the intra-group total hold processing for a nodehaving two child nodes (the processing for holding the estimated noderelative floor height errors of the two child nodes of the n-th nodewithout updating them) is carried out in S6406. In the hold processing,as shown by the expressions in the flowchart of FIG. 48, the values ofthe estimated node relative floor height errors Zi_rel_estm andZj_rel_estm of the i-th node and the j-th node, respectively, aremaintained at the values Zi_rel_estm_p and Zj_rel_estm p at the lastcontrol cycle.

Next, if it is determined in S6400 of FIG. 45 that the modes of the twochild nodes are “all reset,” then intra-group total reset processing fora node having two child nodes (the processing for resetting theestimated node relative floor height error of each of the two childnodes owned by the n-th node) is carried out in S6408. In the resetprocessing, the estimated node relative floor height errors Zi_rel_estmand Zj_rel_estm of the i-th node and the j-th node, respectively, areupdated according to the expressions in the flowchart of FIG. 47 suchthat they gradually approximate to zero. Incidentally, the meanings ofΔT and Testm in the expressions are the same as those of the aforesaidexpression 35.

More generally speaking, the reset processing is the processing in whichZi_rel_estm and Zj_rel_estm are determined to take values that arecloser to zero than Zi_rel_estm_p and Zj_rel_estm_p are, whilesatisfying a condition in which the weighted average value ofZi_rel_estm and Zj_rel_estm is zero, that is,Wi*Zi_rel_estm+Wj*Zj_rel_estm=0. Incidentally, the reset processingincludes a primary delay element, so that infinite time is required forthe node relative floor height errors to completely return to zero.Hence, a finite stabilization function generator previously proposed bythe present applicant (Japanese Unexamined Patent ApplicationPublication No. H5-324115) may be used to gradually reset the noderelative floor height errors to zero.

Further, if the modes of the two child nodes are “others,” that is, ifthe modes are neither “all ready” nor “all reset” in S6400 of FIG. 45(for example, if the modes of the two child nodes are all hold modes),then the same processing as that of S6406 (refer to FIG. 48) is carriedout in S6410.

As described above, only in a case where the modes of the two childnodes of the n-th node are all ready, and there is no danger in that thedivergence takes place in the processing of estimating node relativefloor height errors, the substantial estimation processing of the noderelative floor height errors of the two child nodes is carried out. And,even if the modes of the child nodes are all ready, if there is a dangerof the occurrence of divergence of estimation processing, then thevalues of the estimated node relative floor height errors of the twochild nodes are held. Further, in a situation wherein the actual nodefloor reaction forces of the two child nodes are both zero, that is, ifno actual floor reaction forces act on every ground contact portions 10belonging to an n-th node, then the node relative floor height errors ofthe two child nodes are reset so that they are gradually reset to zero.

Referring now to FIG. 49 to FIG. 55, the three-child-node floorconfiguration estimation processing of S66 of FIG. 41 will be explained.FIG. 49 is a flowchart showing the subroutine processing of thethree-child-node floor configuration estimation processing, FIG. 50 andFIG. 51 are flowcharts showing the subroutine processing of S6604 andS6608 of FIG. 49, FIG. 52 is a flowchart showing the subroutineprocessing of S6606, S6614, and S6618 of FIG. 49, and FIG. 53 and FIG.54 are flowcharts showing the subroutine processing of S6612 and S6616,respectively, of FIG. 49.

In the three-child-node floor configuration estimation processing, asshown in the flowchart of FIG. 49, the modes of all child nodes of ann-th node having three child nodes (the modes determined in S58 of FIG.41) are determined in S6600. The determination results are divided intosix, namely, “all ready,” “all reset,” “only two child nodes are ready,”“only one child node is held, and the rest are reset,” “only two childnodes are held and the rest is reset,” and “others (else).”

Here, in the flowchart of FIG. 49, to explain more generally the floorconfiguration estimation processing for three child nodes,considerations are given not only to a case where all the ground contactportions 10 belonging to each intermediate node (the 145th node and the236th node) simultaneously come in contact with the ground or leave afloor, as explained in the aforesaid second embodiment, but also to acase where a desired gait of a robot is generated such that one of theground contact portions of each intermediate node is in contact with theground, while the remaining ground contact portions leave the floor. Inthis case, unlike the two-child-node type, it is necessary to consideralso when the modes of the three child nodes are “only two child nodesare ready,” “only one child node is held, and the remaining ones arereset,” or “only two child nodes are held, and the remaining are reset.”In the following explanation, the three child nodes of an n-th node willbe referred to as an i-th node, a j-th node, and a k-th node.

Here, if it is determined that the modes of child nodes are “all ready,”then it is determined in S6602 whether a resultant force Fn_z (=Fi_actz+Fj_act_z+Fk_act_z) of the translational force vertical componentsFi_act_z, Fj_act_z, and Fk_act_z of the actual node floor reactionforces of the child nodes of the n-th node is larger than apredetermined value Fn_min2. In other words, Fn_n denotes thetranslational force vertical component of the resultant force of theactual floor reaction forces of all ground contact portions belonging tothe n-th node.

If the determination result of S6602 is YES, then intra-group completeestimation processing for a node having three child nodes (theprocessing for virtually estimating the node relative floor height errorof each of the three child nodes owned by the n-th node) is carried outin S6604. In this processing, estimated node relative floor heighterrors Zi_rel_estm, Zj_rel_estm, and Zk_rel_estm of the i-th node, j-thnode, and the k-th node, respectively, which are the three child nodesof the n-th node, are determined (updated) according to the expressionsshown in the flowchart of FIG. 50. More specifically, Zi_rel_estm willbe representatively explained. An i-th node relative floor height errorcorrection amount candidate value Zi_inc_cand determined in theaforesaid S54 is added to a value Zi_rel_estm_p of Zi_rel_estm in thelast control cycle thereby to determine a new estimated i-th noderelative floor height error Zi_rel_estm. The same applies to the j-thnode and the k-th node.

If the determination result of S6602 is NO, then it means that theaccuracy of estimating a floor configuration error would be excessivelydeteriorated, so that no substantial estimation processing is carriedout, and the intra-group total hold processing for a node having threechild nodes (the processing for holding the estimated node relativefloor height errors of the three child nodes of the n-th node withoutupdating them) is carried out in S6606. In the hold processing, as shownby the expressions in the flowchart of FIG. 52, the values of theestimated node relative floor height errors Zi_rel_estm, Zj_rel_estm,and Zk_rel_estm of the i-th node, the j-th node, and the k-th node,respectively, are maintained at the values Zi_rel_estm_p, Zj_rel_estm_p,and Zk_rel_estm_p at the last control cycle.

Further, if it is determined in S6600 of FIG. 49 that the modes of thethree child nodes are “all reset,” then intra-group total resetprocessing for a node having three child nodes (the processing forresetting the estimated node relative floor height error of each of thethree child nodes owned by the n-th node) is carried out in S6608. Inthe reset processing, the estimated node relative floor height errorsZi_rel_estm, Zj_rel_estm, and Zk_rel_estm of the i-th node, the j-thnode, and the k-th node, respectively, are updated according to theexpressions in the flowchart of FIG. 51 such that they graduallyapproximate to zero. Incidentally, the meanings of ΔT and Testm in theexpressions are the same as those of the aforesaid expression 35.

More generally speaking, the reset processing is the processing in whichZi_rel_estm, Zj_rel_estm, and Zk_rel_estm are determined to take valuesthat are closer to zero than Zi_rel_estm_p, Zj_rel_estm_p, andZk_rel_estm_p are, while satisfying a condition in which their weightedaverage value is zero, that is,Wi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0. Incidentally, theaforesaid finite stabilization function generator (Japanese UnexaminedPatent Application Publication No. H5-324115) may be used to resetZi_rel_estm_p, Zj_rel_estm_p, and Zk_rel_estm_p to zero.

Further, if it is determined in S6600 of FIG. 49 that the modes of thethree child nodes are “only two child nodes are ready,” then it isdetermined in S6610 whether the translational force vertical componentFn_n of the resultant force of the actual node floor reaction forces ofthe child nodes of an n-th node (the resultant force of the actual floorreaction forces of all ground contact portions 10 belonging to the n-thnode) is larger than the predetermined value Fn_min2.

If the result of the determination is YES, then the intra-group partialestimation processing for a node having three child nodes (theprocessing for substantially estimating the node relative floor heighterror of each of the three child nodes owned by the n-th node) iscarried out in S6612. This processing is carried out as shown by theflowchart of FIG. 53. In this case, it is assumed that the i-th node isnot in the ready mode, while the j-th node and the k-th node are in theready mode.

First, in S66120, the mode of the i-th node is determined if the resultof the determination is the hold mode, then a new i-th node relativefloor height error correction amount candidate value Zi_inc_cand′ isdetermined to be zero in S66122, or if it is the reset mode, then thenew i-th node relative floor height error correction amount candidatevalue Zi_inc_cand′ is determined in S66124 according to the expressionin the figure. Zi_inc_cand′ determined in S66124 denotes an i-th noderelative floor height error correction amount candidate value forgradually approximating Zi_rel_estm_p to zero. The meanings of ΔT andTestm in the expressions of S66124 are the same as those of theaforesaid expression 35. Supplementally, the aforesaid finitestabilization function generator (Japanese Unexamined Patent ApplicationPublication No. H5-324115) may be used to determine Zi_inc_cand′ inS66124.

Subsequently, in S66126, a new j-th node relative floor height errorcorrection amount candidate value Zj_inc_cand′ and a new k-th noderelative floor height error correction amount candidate valueZk_inc_cand′ are determined according to the expressions in the figure.

More specifically, Zj_inc_cand′ and Zk_inc_cand′ are determined suchthat they satisfy a condition represented byWi*Zi_inc_cand′+Wj*Zj_inc_cand′+Wk*Zk_inc_cand′=0 (a condition in whichthe weighted average value of Zi_inc_cand′, Zj_inc_cand′ andZk_inc_cand′ is zero) and a condition represented byZj_inc_cand′−Zj_inc_cand=Zk_inc_cand′−Zk_inc_cand.

Subsequently, in S66128, Zi_inc_cand′, Zj_inc_cand′, and Zk_inc_cand′determined as described above are added to the values Zi_rel_estm_p,Zj_rel_estm_p, and Zk_rel_estm_p of the relative floor height errors ofthe i-th node, the j-th node, and the k-th node at the last controlcycle so as to determine new Zi_rel_estm, Zj_rel_estm, and Zk_rel_estm.

By determining Zi_rel_estm, Zj_rel_estm, and Zk_rel_estm as describedabove Zi_rel_estm, Zj_rel_estm, and Zk_rel_estm will be determined suchthat Zj_rel_estm−Zk_rel_estm approximates to Zj_inc_cand−Zk_inc_candwhile satisfying Wi*Zi_rel_estm+Wj*Zj_rel_estm+Wk+Zk_rel_estm=0 at thesame time.

If the result of the determination of S6610 of FIG. 49 is NO, then theaccuracy of estimating a floor configuration error would be excessivelydeteriorated, so that no substantial estimation processing is carriedout, but the same processing as that of the aforesaid S6606 (refer toFIG. 52) is carried out in S6614.

Further, if it is determined in S6600 of FIG. 49 that the modes of thethree child nodes are “only one child node is held and the rest arereset,” then the processing for a case where only one child node is holdand the rest are reset is carried out in S6616. In the processing, newnode relative floor height errors Zi_rel_estm, Zj_rel_estm, andZk_rel_estm are determined according to the expressions shown in theflowchart of FIG. 54. Incidentally, in this case, it is assumed that themode of the i-th node is the hold mode and the modes of the j-th nodeand the k-th node are the reset mode. The meanings of ΔT and Testm inthe expressions are the same as those in the aforesaid expression 35.

The processing of FIG. 54 is, more generally, the processing fordetermining Zi_rel_estm to be zero, and determining Zj_rel_estm andZk_rel_estm to take values that are closer to zero than Zj_rel_estm_pand Zk_rel_estm_p are, while satisfyingWi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0 (a condition in which theweighted average value of Zi_rel_estm, Zj_rel_estm and Zk_rel_estm iszero) . Supplementally, by the moment the modes of the j-th node and thek-th node both become the reset mode, their node weights Wj and Wkshould have become zero. Hence, based on a condition in that the sum ofthe weights Wi, Wj and Wk is 1, Wi will be 1 by that moment andWi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0, so that Zi_rel_estm willbe zero by that moment. Thus, holding the value of the i-th nodeZi_rel_estm means maintaining the value at zero. Therefore, in theprocessing of FIG. 54, the value of Zi_rel_estm is determined to bezero.

Incidentally, the aforesaid finite stabilization function generator(Japanese Unexamined Patent Application Publication No. H5-324115) maybe used in the processing for gradually resetting Zj_rel_estm andZk_rel_estm to zero.

Further, if it is determined in S6600 of FIG. 49 that the modes of thechild nodes are “only two child nodes are held and the rest is reset,”then the processing for a case where only two child nodes are hold andthe rest is reset is carried out in S6618. In the processing, new noderelative floor height errors Zi_rel_estm, Zj_rel_estm, and Zk_rel_estmare determined according to the expressions shown in the flowchart ofFIG. 55. Incidentally, in this case, it is assumed that the modes of thei-th node and the j-th node are both the hold mode and the mode of thek-th node is the reset mode. The meanings of ΔT and Testm in theexpressions are the same as those in the aforesaid expression 35.

The processing of FIG. 55 is, more generally, the processing for holdingZi_rel_estm and Zj_rel_estm at the values at the last control cycle, anddetermining Zk_rel_estm to take a value that is closer to zero thanZk_rel estm_p is, while satisfyingWi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0 (a condition in which theweighted average value of Zi_rel_estm, Zj_rel_estm and Zk_rel_estm iszero). Supplementally, by the moment the mode of the k-th node becomesthe reset mode, Wk should have become zero.

Incidentally, the aforesaid finite stabilization function generator(Japanese Unexamined Patent Application Publication No. H5-324115) maybe used in the processing for gradually resetting Zk_rel_estm to zero.

Further, if it is determined to be “Others” in S6620 of FIG. 49 (e.g.,if the modes of the three child nodes are all hold mode), then the sameprocessing as that of the aforesaid S6606 (refer to FIG. 52) is carriedout in S6620.

As described above, only in a case where the modes of two or more childnodes are ready out of the modes of the three child nodes of the n-thnode, and there is no danger in that the divergence takes place in theprocessing of estimating node relative floor height errors, thesubstantial estimation processing of the node relative floor heighterrors of the two or more child nodes is carried out. And, even if themodes of two or more child nodes are all ready, if there is a danger ofthe occurrence of divergence of estimation processing, then the valuesof the estimated node relative floor height errors of the three childnodes are held.

As described above, in S60 to S66 of FIG. 41, estimated n-th relativefloor height errors Zn_rel_estm(n=1, 2, . . . , last node number) aredetermined. Incidentally, the relative floor height error of the rootnode is zero.

In the processing of the flowchart of FIG. 41, lastly, in S72, the sumof the estimated relative floor height errors of all ancestor nodes ofan n-th node (n=1, 2, . . . , last leaf node number) and an estimatedn-th node relative floor height error is determined, and the determinedsum is defined as an estimated n-th ground contact portion floor heighterror (estimated n-th floor height error) Zfn_estm.

The above is the subroutine processing of the estimation processing offloor height errors in FIG. 40.

Next, returning to the explanation of FIG. 35, the estimated n-th floorheight error Zfn_estm(n=1, 2, . . . , last leaf node number)sequentially determined by the floor configuration estimator 130 asdescribed above is added to the corrected desired ground contact portionposition/posture of each ground contact portion 10 by an adder 132 so asto determine corrected desired ground contact portion position/posturewith floor configuration error compensation. Then, the corrected desiredground contact portion position/posture with floor configuration errorcompensation is input, in place of corrected desired ground contactportion position/posture, to the corrected desired ground contactportion position/posture with deformation compensation calculator 114 h.

The processing of the control device 50 including the floorconfiguration estimator 130 and the adder 132 explained above isrepeatedly carried out at each control cycle of the control device 50,the estimated n-th floor height error Zfn_estm converges to an actualn-th floor height error. Further, desired ground contact portionpositions/postures are corrected on the basis of the estimated n-thfloor height errors Zfn_estm corresponding to the ground contactportions so as to absorb an influence exerted by an actual n-th floorheight error on an actual floor reaction force.

If all n-th ground contact portions (n=1, 2, . . . , last leaf nodenumber) are in contact with the ground, then the point corresponding toa desired n-th ground contact portion floor reaction force central pointQn on the actual ground contact surface of an n-th ground contactportion should agree with an actual n-th floor contact point Dn_act.Therefore, the aforesaid instantaneous n-th node relative floor heighterror Zn_rel′ should not change. In actuality, however, theinstantaneous n-th node relative floor height error Zn_rel′ obtained bythe aforesaid calculation algorithm of the aforesaid floor configurationestimator 130 significantly changes due to the vibrations and electricalnoises of a mechanical system or control device of the robot 1.Therefore, if the instantaneous n-th node relative floor height error isdirectly used as an estimated n-th node relative floor height error, andbased on this, the estimated n-th floor height error Zfn_estm isdetermined (that is, the sum of the instantaneous relative floor heighterrors of all ancestor nodes of an n-th node (n=1, 2, . . . , last leafnode number) and the instantaneous n-th node relative floor height erroris defined as the estimated n-th floor height error Zfn_estm), andfurther, the result obtained by adding the estimated n-th floor heighterror Zfn_estm to the corrected desired ground contact portionposition/posture is supplied to the corrected desired ground contactportion position/posture with deformation compensation calculator 114 h,as shown in FIG. 35, then the ground contact portions 10 of the robot 1will oscillate or severely vibrate.

Meanwhile, in the intra-group complete estimation processing for a nodehaving two child nodes, the intra-group complete estimation processingfor a node having three child nodes, and the intra-group partialestimation processing for a node having three child nodes describedabove, the transfer function from the instantaneous n-th node relativefloor height error Zn_rel′ to the estimated n-th node relative floorheight error Zn_rel_estm will be a primary delay low-pass filter. Inother words, the estimated n-th node relative floor height errorZn_rel_estm will be the one obtained by passing the instantaneous n-thnode relative floor height error Zn_rel′ through a low-pass filter.Therefore, even when the estimated n-th floor height error Zfn_estmdetermined on the basis of the estimated n-th node relative floor heighterror Zn_rel_estm is added to the corrected desired ground contactportion position/posture, as shown in FIG. 35, the oscillation orvibration of the ground contact portions 10 will hardly occur. Thismakes it possible to restrain the ground contact portions 10 fromoscillating or vibrating.

Incidentally, in the present embodiment, the time constant of theaforesaid low-pass filter is Testm. Other than by the processing of thefloor configuration estimator 130 explained in the present embodiment,it is possible to accomplish a construction in which the transferfunction from the instantaneous n-th node relative floor height errorZn_rel′ to the estimated n-th node relative floor height errorZn_rel_estm will be a low-pass filter.

The characteristics of the floor configuration estimator 130 in thepresent embodiment will now be explained.

Even if each node compensating angle is changed and the correcteddesired ground contact portion position/posture with deformationcompensation, which are the final desired ground contact portionposition/posture, are changed to generate node compensating momentsrequired for the stabilization control of the posture of the robot 1,the point corresponding to the desired n-th ground contact portion floorreaction force central point Qn of the n-th ground contact portion 10 onan actual ground contact surface remains in agreement with the actualn-th floor contact point Dnact as long as the ground contact portions 10are actually in contact with the ground. Instead, the compliancemechanism 42 or the like of each leg deforms or the position/posture ofthe body 24 change. In other words, the compliance mechanism 42 or thelike or the position/posture of the body 24 change in order to cancelthe change of the corrected desired ground contact portionposition/posture with deformation compensation.

The floor configuration estimator 130 in the present embodiment cancelsthe influences exerted on the estimated n-th floor height error Zfn_estmby the deformation amount of the compliance mechanism 42 or the like,which is calculated by the mechanism compliance model 134 on the basisof an actual floor reaction force, and the body posture inclinationerror θberr by the time the estimated n-th floor height errorZfn_estm(n=1, 2, . . . , last leaf node number) is calculated from thecorrected desired ground contact portion position/posture withdeformation compensation. Hence, even if a body posture inclinationerror occurs or corrected desired ground contact portionposition/posture with deformation compensation change, the estimatedn-th floor height error Zfn_estm is not influenced thereby.

This means that the processing for estimating a floor configuration isnot subjected to interference by the compliance control or posturecontrol of the robot 1. Therefore, even when the estimated n-th floorheight error Zfn_estm is added to the corrected desired ground contactportion position/posture, as shown in FIG. 35, the stability allowance(resistance to oscillation) of the control device of the robot 1 hardlyreduces. In other words, control and processing will not interfere witheach and cause oscillation even if the compliance control, posturecontrol and the floor configuration estimation processing and theoperation for correcting a desired gait of the robot 1 by using theestimated value of a floor configuration error are simultaneouslyperformed. This means, in brief, that the leg compensating operation tocancel the influence exerted on a floor reaction force by a floorconfiguration error (the operation for correcting the position/postureof a ground contact portion) can be simultaneously performed whilesimultaneously estimating a plurality of parameters of the floorconfiguration error. Moreover, the leg compensating operation forposture control (the operation for correcting the position/posture of aground contact portion) can be simultaneously performed.

Further, if the estimated n-th floor height error Zfn_estm of eachground contact portion 10 is added to the corrected desired groundcontact portion position/posture, as shown in FIG. 35, then even ifthere is actually an n-th floor height error, the influence thereof canbe offset, so that the actual total floor reaction force will steadilybe the same as that in a case where the robot 1 is traveling on asupposed floor. Furthermore, the estimation of the estimated n-th floorheight error Zfn_estm and the correction of corrected desired groundcontact portion position/posture by using the estimated n-th floorheight error Zfn_estm are sequentially carried out for each controlcycle; therefore, even if a floor configuration changes on the way, then-th floor height error after the change can be estimated and theinfluence exerted by the change in the floor configuration can becancelled on the basis of the estimated value. However, the floorconfiguration estimator 130 includes a low-pass filter, as mentionedabove, so that an actual total floor reaction force is transitionallysubject to the influence of a floor configuration error immediatelyafter the ground contact portions 10 come in contact with the ground orif the floor configuration changes on the way. Thereafter, however, theinfluence is attenuated by the time constant Testm.

Further, there are the following characteristics.

-   1) Control is resistant to failure even if a floor configuration    error is large. This is because the hierarchical compliance control    does not carry out approximation limited to minute compensating    angles.-   2) The accuracy of estimating floor configuration errors is high.-   3) Resistance to the occurrence of oscillation attributable to the    interference with the robot posture stabilization control,    permitting a shorter time constant for estimation processing to be    achieved. This makes it possible to enhance the responsiveness of    the leg compensating operation to offset the influence exerted by a    floor configuration error on a floor reaction force, so that the    transitional influence exerted by the floor configuration error on    the floor reaction force can be quickly eliminated.

If there is no correlation in the relationship between the estimatedn-th floor height error in a landing period of one step before (a periodin which a certain ground contact portion 10 comes in contact with theground) and the estimated n-th floor height error in a landing periodfollowing the above landing period of one step before when a robot istraveling, then it is preferred to forcibly and gradually converge theestimated n-th floor height error to zero, as shown in the presentembodiment. However, if, for example, it is known that the undulation ofan actual floor surface not considered in a supposed floor is gentle,then it is considered that there is some correlation in the aforesaidrelationship. In this case, the estimated n-th floor height error in thecurrent landing period may be forcibly and gradually converged to thevalue obtained by multiplying the estimated n-th floor height error inthe landing period of one step before by a positive constant that issmaller than 1.

Further, the convergence desired value of an estimated n-th floor heighterror in the current landing period may be determined by using not onlythe estimated n-th floor height error in the landing period of one stepbefore but also the estimated n-th floor height error in the landingperiod of a plurality of steps before. Further, the convergence desiredvalue may be determined by using the estimated n-th node floor heighterror and the estimated n-th node floor inclination error of anothernode in the landing period of current step, one step before or aplurality of steps before.

Since the present embodiment (the third embodiment) has been constructedas described above, the estimation of floor configurations,specifically, the floor height errors associated with ground contactportions, can be simultaneously, in other words, compositely, estimatedwith high accuracy even in a robot having many ground contact portions,which has been difficult to accomplish with conventional technologies.Moreover, if the ground contact portions of a robot are provided suchthat their postures can be controlled, and floor inclination errors areestimated in addition to floor height errors in the processing forestimating floor configurations, the floor inclination errors can bealso simultaneously estimated with high accuracy.

Furthermore, by correcting a desired ground contact portionposition/posture trajectory on the basis of the result of estimating afloor configuration, even if a floor configuration is different from asupposed configuration, the influence thereof can be absorbed, making itpossible to generate a floor reaction force as desired.

In particular, the steady error of an actual floor reaction force from acontrol desired value, which has not been completely removed by thehierarchical compliance control that does not include corrections basedon the estimated values of floor configuration errors, can beapproximated to zero as much as possible. In other words, the steadyerrors of floor reaction forces caused by floor configuration errors canbe eliminated.

Furthermore, the arrangement has been made such that the estimation of afloor configuration is interrupted in a situation wherein the accuracyof estimating a floor configuration may deteriorate or a situationwherein the estimated value thereof may diverge. This adds to theaforesaid advantages by preventing estimated values from beinginappropriate.

Incidentally, the floor configuration estimator 130 of a robot accordingto the present embodiment is expected to estimate floor configurationsby the aforesaid techniques, but it is not essential for the floorconfiguration estimator 130 to correct motions on the basis of estimatedvalues.

FOURTH EMBODIMENT

A control device (a floor configuration estimator in particular) of amobile robot according to a fourth embodiment of the present inventionwill now be explained. In the fourth embodiment, to explain it in brief,the block diagram of the floor configuration estimator 130 shown in FIG.40 of the aforesaid third embodiment has been equivalently converted.More specifically, the floor reaction force of each node is estimated onthe basis of the estimated value of a floor configuration estimatedimmediately before (the past value of an estimated value, such as theestimated value in the last control cycle), a desired gait that has beenfinally corrected and determined (detailedly, corrected desired groundcontact portion position/posture with deformation compensation includinga compliance operation)(or an actual joint displacement), and an actualbody posture inclination error θberr. Based on the difference betweenthe estimated value and the actual floor reaction force of each node,the correction amount candidate value of the estimated value of thefloor configuration is determined to approximate the difference to zero.If it is determined that the estimation of the floor configuration willnot diverge, then the estimated value of the floor configuration thathas been estimated immediately before is corrected on the basis of thecorrection amount candidate value of the estimated value of the floorconfiguration. This processing corresponds to a hierarchical floorconfiguration estimating means that is an advanced version of themeaning for estimating a floor inclination related to each foot suchthat the difference between the supposed moment about the floor reactionforce central point of the foot of each leg of a biped mobile robot andan actual moment approximates to zero and the means for estimating theinterference angle between both legs such that the difference betweenthe supposed moment about a total floor reaction force central point andan actual moment approximates to zero in the Japanese Unexamined PatentApplication Publication No. H10-277969 previously proposed by thepresent applicant.

Thus, the present embodiment differs from the third embodiment only inthe processing of a floor configuration estimator 130, so that theexplanation of the processing of those other than the floorconfiguration estimator 130 will be omitted.

FIG. 57 is a block diagram showing the functional means of the floorconfiguration estimator 130 in the present embodiment. Referring to thisFIG. 57, the floor configuration estimator 130 in the present embodimentwill be explained below.

First, based on corrected desired n-th ground contact portionposition/posture with deformation compensation (n=1, 2, . . . , lastleaf node number) of each ground contact portion 10, the desired n-thground contact portion floor reaction force central point of the groundcontact portion 10 observed from the desired ground contact portionposition of each ground contact portion 10, and the aforesaid postureinclination error θberr, the corrected desired n-th ground contactportion position/posture with deformation compensation is rotationallymoved by θberr, about a desired total floor reaction force central pointbeing the center of the rotation, so as to determine the correcteddesired n-th ground contact portion position/posture (n=1, 2, . . . ,last leaf node number) with deformation compensation after rotation. Asupposed n-th floor contact position Dn(n=1, 2, . . . , last leaf nodenumber) on a desired gait is subtracted from the determined correcteddesired n-th ground contact portion position/posture (n=1, 2, . . . ,last leaf node number) with deformation compensation after rotation soas to determine an n-th ground contact portion interference heightZn_int. This is supplied to a mechanism compliance model (inverse model)provided in a floor configuration estimator 130, thereby determining anestimated n-th ground contact portion floor reaction force, which is theestimated value of the floor reaction force of each ground contactportion 10.

Incidentally, the mechanism compliance model here is used to determinethe mutual relative relationship among the estimated ground contactportion floor reaction forces on the basis of the mutual relativerelationship among ground contact portion interference heights, and itfinds no meaning in absolute values. Further, if the detected value ofan actual floor reaction force Fn_act of an n-th ground contact portionis smaller than a certain threshold value Fn_min3, then it is assumedthat the n-th ground contact portion is not in contact with the groundalso in the aforesaid mechanism compliance model.

Further, the aforesaid estimated n-th ground contact portion floorreaction force is subtracted from the detected value of the actual floorreaction force Fn_act of an n-th ground contact portion (n=1, 2, . . . ,last leaf node number) to determine the estimated error of the n-thground contact portion floor reaction force Ffn_estm_err. The estimatederror of the n-th ground contact portion floor reaction forceFfn_estm_err is expressed in terms of a force, so that it is convertedinto a height error by a conversion value Cn (e.g., the reciprocal of aspring constant) and the result is adopted as an n-th ground contactportion floor height error correction amount candidate valueZfn_inc_cand. Incidentally, the conversion value Cn is not necessarily adiagonal matrix.

Subsequently, hierarchical relativization is performed by thehierarchical relativization processing described above on the basis ofthe determined n-th ground contact portion floor height error correctionamount candidate value Zfn_inc_cand so as to determine an n-th noderelative floor height error correction amount candidate valueZn_inc_cand(n=1, 2, . . . , last node number). Then, based on the n-thnode relative floor height error correction amount candidate valueZn_inc_cand, an estimated floor height error (estimated n-th floorheight error) Zfn_estm(n=1, 2, . . . , last leaf node number) isdetermined by the subroutine processing of the estimation processing offloor height errors. The subroutine processing of the estimationprocessing of floor height errors here is identical to the processing ofS56 to S72 shown in FIG. 41.

It is desirable to carry out the ground contact portion floorinclination error estimation processing as explained in conjunction withS70 of FIG. 41 in the case of a robot that has feet whose postures canbe controlled as ground contact portions and is capable of estimatingthe floor inclination errors in ground contact portions, as disclosed inJapanese Unexamined Patent Application Publication No. H10-277969.

Here, the third embodiment and the fourth embodiment (the presentembodiment) will be compared. The fourth embodiment is, in brief, theconverted equivalent of the block diagram of the floor configurationestimator 130 of the third embodiment. Hence, the advantages of thefourth embodiment are the same as the advantages of the thirdembodiment. Further, as with the third embodiment, it is not essentialto correct motions on the basis of the estimated values of floorconfigurations.

In the third and the fourth embodiments, distributed pressure sensorsmay be used as floor reaction force sensors. Distributed pressuresensors make it possible to estimate more finely the deformations ofground contact portions due to actual floor reaction forces than forcesensors, such as the six-axis force sensor 34, so that the accuracy inestimating floor configurations can be improved.

Furthermore, as shown in FIG. 1 of Japanese Patent No. 3035051 by thepresent applicant, a plurality of contact sensors may be disposed on aground contact portion (e.g., on the four corners of the ground contactportion), and it may be determined in which direction the ground contactportion is likely to float on the basis of the output signals thereof.Then, the estimation of a floor configuration in the direction in whicha contact sensor that is not in contact with the ground is furtherlikely to float may be interrupted, or the estimation of a floorconfiguration may be also interrupted when all contact sensors arefloating.

FIFTH EMBODIMENT

In the first to the fourth embodiments described above, the explanationshave been given to the cases where the total number of child nodes of anode having child nodes is three or less have been taken as examples;however, also in a case where the number of child nodes is four or more,the concepts of the moments and inclination angles can be expanded asshown below by introducing the concept of a weighted average.

The concept can be expanded for any of desired values, actual values,and errors. Here, therefore, the desired values, the actual values, anderrors will not be discriminated; instead, the moment for a general setof ground contact portion floor reaction forces Fn(n=1, 2, . . . ) willbe expanded and defined as shown below.

A set of the floor reaction forces of an n-th ground contact portionFn(n=1, 2, . . . , last leaf node number) is hierarchically relativizedto determine an n-th node relative floor reaction force Fn_rel(n=1, 2, .. . , last node number).

The number of the child nodes of an n-th node is denoted by r.

The identification number of an j-th child node of the n-th node isdenoted by aj (j=1, 2, . . . , r).

The weight of the j-th child node of the n-th node is denoted by Wj(j=1, 2, . . . , r).

The column vector having, as a j-th element, the weight of the j-thchild node of the n-th node is denoted by UWn.

Thus, UWn is defined as shown by the following expression 38, where “T”means transposition, and a column vector can be expressed by transpose arow vector.UWn=(Wa1, Wa2, . . . , War)^(T)  Expression 38

r−1 mutually independent vectors orthogonal to the vector UWn (that is,the scalar product with the vector UWn is zero) are denoted byR(1),R(2), . . . , R(r−1). R(j) (j=1, 2, . . . ,r−1) denotes the columnvector of r row, 1 column. Incidentally, R(j) (j=1, 2, . . . , r−1) aredesirably orthogonal to each other, considering the ease of computationand control accuracy. A matrix having R(j) set to a j-th column (j=1, 2,. . . , r−1) is denoted by Hn. Hn is the matrix of r row, r−1 column.

A column vector whose j-th element (j=1, 2, . . . ,r) is a relativefloor reaction force Faj_rel of a j-th child node of the n-th node isdefined as an n-th group relative floor reaction force vector Fn_rel_c.

In other words, Fn_rel_c is defined by the following expression 39.Fn_rel_c=(Fa1_rel, Fa2_rel, . . . , Far_rel)^(T)  Expression 39Fn_rel_c denotes a column vector of r row, 1 column.

A column vector Mn_exp satisfying the following expression 40 isreferred to as an n-th node expansion floor reaction force moment.Fn _(—) rel _(—) c=Hn*Mn_exp  Expression 40Mn_exp denotes a column vector of r−1 row, 1 column.

For the set of ground contact portion floor reaction forces Fn(n=1, 2, .. . ), the n-th node expansion floor reaction force moment Mn_exp isdefined as described above.

More specifically, based on the ground contact portion floor reactionforce Fn(n=1, 2, . . . ), a hierarchized node relative floor reactionforce Fn_rel(n=1, 2, . . . ) is determined, and further, a vectorhaving, as its element, the coefficient of the linear combination when avector (Fa1_rel,Fa2_rel, . . . ,Far_rel)^(T) having, as its elements,the relative floor reaction forces of all child nodes aj (j=a1,a2, . . .,ar) of an n-th node is expressed by the liner combination of certainpredetermined mutually independent vectors R(j)(j=1, 2, . . . , r−1)that are orthogonal to a vector (Wa1,Wa2, . . . ,War)^(T) having, as itselements, the weights of all child nodes of the n-th node is referred toas the n-th node expansion floor reaction force moment Mn_exp.

Incidentally, a j-th element (the element of a j-th row) of the n-thnode expansion floor reaction force moment Mn_exp is referred to as thej-th component of the n-th node expansion floor reaction force moment.

Further, the n-th node expansion floor reaction force moment relative tothe set of actual ground contact portion floor reaction forces Fn(n=1,2, . . . ) is referred to as the actual n-th node expansion floorreaction force moment Mn_exp_act.

Further, the n-th node expansion floor reaction force moment relative tothe set of desired ground contact portion floor reaction forces Fn(n=1,2, . . . ) is referred to as the desired n-th node expansion floorreaction force moment Mn_exp_ref. The desired n-th node expansion floorreaction force moment is usually set to zero vector.

Further, the n-th node expansion floor reaction force moment relative tothe set of ground contact portion floor reaction force errors Fn(n=1, 2,. . . ) is referred to as the n-th node expansion floor reaction forcemoment error Mn_exp_err.

Similarly, the inclination angle relative to a set of general groundcontact portion heights Zn(n=1, 2, . . . ) is expanded and defined asfollows.

A set of ground contact portion heights Zn(n=1, 2, . . . , last leafnode number) is hierarchically relativized to determine an n-th noderelative height Zn_rel(n=1, 2, . . . , last node number).

A column vector whose j-th element (j=1, 2, . . . , r) is a relativeheight Zaj_rel of a j-th child node of the n-th node is defined as ann-th node relative height vector Zn_rel_c.

In other words, Zn_rel_c is defined by the following expression 41.Zn_rel_c=(Za1_rel, Za2_rel, . . . , Zar_rel)^(T)  Expression 41

A column vector θn_exp satisfying the following expression 42 isreferred to as an n-th node expansion floor reaction force inclinationangle.Zn _(—) rel _(—) c=Hn*θn_exp  Expression 42

θn_exp denotes a column vector of r−1 row, 1 column.

More specifically, based on the ground contact portion height Zn(n=1, 2,. . . ), a hierarchized node relative height Zn_rel(n=1, 2, . . . ) isdetermined, and further, a vector having, as its element, thecoefficient of the linear combination when a vector (Za1_rel,Za2_rel, .. . ,Zar_rel)^(T) having, as its elements, the relative heights of allchild nodes aj (j=1, 2, . . . , r) of an n-th node is expressed by theliner combination of certain predetermined mutually independent vectorsR(j)(j=1, 2, . . . , r−1) that are orthogonal to a vector (Wa1,Wa2, . .. ,War)^(T) having, as its elements, the weights of all child nodes ofthe n-th node is referred to as the n-th node expansion inclinationangle θn.

By using the expansion floor reaction force moment and the expansioninclination angle defined as described above, the compliance control andthe processing for estimating floor configurations are expanded almostas they are for a case where the number of the child nodes of an a-thnode is four or more. The following will explain an example thereof as afifth embodiment. In the fifth embodiment, the functional constructionof a control device of a robot is the same as that shown in theaforesaid FIG. 2. In this case, the functional construction of ahierarchical compliance operation determiner may be the same as thatexplained with reference to the aforesaid FIG. 35 except for acompensating total floor reaction force moment distributor, acompensating angle determiner, and a floor configuration estimator.

[Expanding the Processing of the Compensating Total Floor Reaction ForceMoment Distributor]

For example, the processing of the compensating total floor reactionforce moment distributor in the hierarchical compliance operationdeterminer is expanded as shown below.

The expansion floor reaction force moment to be generated in addition toa desired n-th node expansion floor reaction force moment Mn_exp_rel(normally zero) to restore the posture of a robot is referred to as ann-th node compensating expansion floor reaction force moment Mn_exp_dmd.

Expression 43 given below holds between the n-th node compensatingexpansion floor reaction force moment Mn_exp_dmd and the n-th nodecompensating floor reaction force moment Mn_dmd. However, Cn_mechdenotes a predetermined coefficient matrix (the matrix of 2 rows, r−1column) determined by a desired node floor reaction force central pointand the compliance characteristic of the mechanism of the robot.Mn _(—) dmd=Cn_mech*Mn_exp_(—) dmd  Expression 43

Meanwhile, generalizing the aforesaid expression 10 provides thefollowing expression 44.Mdmd=ΣMn_dmd  Expression 44

where Σ means the total sum on all n-th nodes.

Hence, the compensating total floor reaction force moment distributormay determine the n-th node compensating expansion floor reaction forcemoment Mn_exp_dmd (n=1, 2, . . . ) and the n-th node compensating floorreaction force moment Mn_dmd such that expression 43 and expression 44are satisfied.

[Expanding the Processing of the Compensating Angle Determiner (θnDeterminer)]

The processing of the n-th node compensating angle determiner in thefifth embodiment is carried out as follows. For example, in theprocessing of the θ145 determiner in the third embodiment (refer to FIG.25 and FIG. 33), the processing until M145act is determined is replacedby the processing for determining the aforesaid actual n-th nodeexpansion floor reaction force moment Mn_exp_act on the basis of theactual floor reaction force of each ground contact portion, 145 isreplaced by n, and the dimension of the compensation filter, thelow-pass filter, and gain Kn in FIG. 33 may be expanded to the numberobtained by subtracting 1 from the number of child nodes of an n-thnode. The compensating angle θn of the n-th node can be determined bythe processing that has been replaced as described above.

[Estimating a Floor Configuration by Using an Expansion Floor ReactionForce Moment]

As described below, the processing of the floor configuration estimatormay partly incorporate a technique using the concept of the expansionfloor reaction force moment.

In place of the processing shown in FIG. 56 of the floor configurationestimator explained in the aforesaid fourth embodiment, the processingshown in the block diagram of FIG. 57 is used to estimate floorconfiguration errors.

Referring to FIG. 57, this processing will be explained. First, an n-thnode expansion floor reaction force moment estimation errorMn_exp_estm_err corresponding to an n-th ground contact portion floorreaction force estimation error Ffn_estm_err (n=1, 2, . . . ) isdetermined according to the definition of the expansion moment from thedifference between the estimated n-th ground contact portion floorreaction force calculated as explained in conjunction with the aforesaidFIG. 56 and the actual floor reaction force of the n-th ground contactportion (the set of differences for each node). Incidentally, theaforesaid vector R(j) required to determine the n-th node expansionfloor reaction force moment estimation error is determined on the basisof the weight of each node.

Subsequently, as shown in expression 45 given below, the n-th nodeexpansion floor reaction force moment estimation error Mn_exp_estm_erris multiplied by a predetermined coefficient matrix Kn_cmpl thereby todetermine an n-th node expansion inclination angle correction amountcandidate value θn_inc_cand, which is the candidate value of thecorrection amount of the n-th node expansion inclination angle.θn _(—) inc _(—) cand=Kn _(—) cmpl*Mn _(—) exp _(—) estm _(—)err  Expression 45

Incidentally, Kn_cmpl is not necessarily a diagonal matrix.

Subsequently, a coefficient for a vector R(j) (j=1, 2, . . . r−1) isdefined as the j-th element of θn_inc_cand to determine a vectorobtained by linearly combining the vectors R(j). The determined vectoris defined as the n-th node relative floor height error correctionamount candidate value Zn_inc_cand, which is the candidate value of thecorrection amount of the n-th node relative floor height error.

In other words, Zn_inc_cand is determined according to the followingexpression 46.Zn _(—) inc _(—) cand=Hn*θn _(—) inc _(—) cand  Expression 46

As described above, the processing for determining the n-th node floorreaction force estimation error Fn_estm_err to the n-th node relativefloor height error correction amount candidate value Zn_inc_cand may bereplaced from the one shown in FIG. 56 to the one shown in FIG. 57.

The arithmetic processing of FIG. 57 other than the above is the same asthe arithmetic processing of FIG. 56.

The subroutine processing of the floor height error estimationprocessing in the fifth embodiment requires the processing for a casewhere the number of child nodes of an n-th node is four or more. Thisprocessing expands the aforesaid floor configuration estimationprocessing for three child nodes (refer to FIG. 41), and if the groundcontact portion corresponding to a child node j is likely to float anddiverge, then the estimated j-th node relative floor height errorZj_rel_estm is maintained at the last value Zj_rel_estm_p (the j-th noderelative floor height error correction amount candidate valueZj_inc_cand is set to zero), as with the floor configuration estimationprocessing for three child nodes. Further, the estimated node relativefloor height errors Zk_rel_estm of the remaining child nodes aredetermined such that they approximate to the value obtained by adding acertain predetermined bias value c to the sum of the last estimated k-thnode relative floor height error Zk_rel_estm_p and Zn_inc_cand. However,the bias value c is determined such that the weighted average of theestimated node relative floor height errors of all child nodes of then-th node is zero.

As described above, the floor configuration estimation processing can beexpanded also when there is a node having four or more child nodes.However, the arithmetic processing will be easier if a hierarchicalstructure is determined not to include four or more child nodes ratherthan carrying out such processing.

SIXTH EMBODIMENT

Referring now to FIG. 58 to FIG. 65, a control device for a mobile robotaccording to a sixth embodiment of the present invention will beexplained. FIG. 58 and FIG. 59 are side views showing a legged mobilerobot 51 according to the sixth embodiment (a bipedal mobile robot inthe present embodiment), the portion of a knee joint 56, which will bediscussed later, being in contact with a floor (kneeling). Incidentally,FIG. 58 schematically shows the robot 51 as a linear link assembly.

An object of the invention related to the present embodiment is tostably control the posture of a robot by manipulating a reaction force(external force) received from a floor, including an object fixed on thefloor, in a state wherein a portion, such as a knee, an elbow, or atorso other than the distal portions of the legs or arms of the robot isin contact with the floor or the object fixed (set) on the floor (anobject regarded as a material extended from the floor).

More specifically, the object is to permit posture control in a statewherein the robot is on its knees or in a state wherein it is sitting ona chair.

A conventional publicly known human-like robot is not provided withsensors for detecting floor reaction forces on, for example, its knees,so that it has been impossible to control the floor reaction forcesacting on the knees. For instance, in the conventional human-like robot,when the robot is on its knees as shown in FIG. 58 and FIG. 59, thefloor reaction forces acting on the right and left knees are dependenton (governed by) the concavities and convexities of a floor, and thejoint angles of legs have not been adjusted on the basis of theconcavities and convexities of the floor. Hence, there have been somecases where the portions of the robot in contact with the floor shakedue to concavities and convexities of the floor, leading to unstableground contact states of the ground contact portions. And, in such astate, an attempt to make the robot move or work sometimes has causedinstability of the posture of the robot.

Further, when, for example, the robot stands up from the state whereinit is kneeling, there has been a danger of the robot falling down if anactual floor inclination is different from the inclination supposed in adesired gait, because the position of the center-of-gravity of the robotis not properly controlled and the robot tries to stand up with its bodyposture inclined.

Further, in a case where the robot sitting on a chair operates or workswhile maintaining its posture stable, it is preferred that not only thefloor reaction forces acting on the feet of the legs of the robot butalso the reaction forces acting from the chair onto the buttocks of therobot can be properly controlled so as to restore the posture of therobot to a proper posture when it stands up from the chair or sits ontothe chair.

Conceptually, if an object, such as a chair, which is fixedly set on afloor is considered as a part of a floor, then the aforesaid reactionforces may be said to be floor reaction forces in a broad sense.

With the view of the problems with the conventional device as describedabove, the sixth embodiment solves the aforesaid problems, and in astate wherein portions of a mobile robot, such as the knees, elbows, thetorso, and buttocks, other than the distal portions of legs or arms arein contact with a floor or an object considered as a material extendedfrom the floor and subjected to reaction forces, it properly controlsnot only the reaction forces acting on the distal portions of the legsor arms of a robot but also the reaction forces acting on the portionsother than the distal portions of the legs or arms, while using thecontrol of the hierarchical compliance described above, thereby makingit possible to keep the posture of the mobile robot stable.

The following will explain the sixth embodiment in more detail by takingthe robot (bipedal mobile robot) 51 shown in FIG. 58 and FIG. 59 as anexample.

First, the mechanical construction of the robot 51 will be explained. Inthe robot 51, two legs (link mechanisms) 52 (one leg being not shown)are extended from the bottom end of a body (base) 53. Further, two arms(link mechanisms) 54 (one arm being not shown) are extended from upperpart of the body 53. Each leg 52 is provided with a hip joint 55, a kneejoint 56, and an ankle joint 57 at its proximal portion adjacent to thebody 53 (the portion connected to the body 53), its middle portion, andits distal portion, respectively, which are operated by actuators, suchas electric motors, and a foot 58 is connected to the distal portion ofeach leg 52 through the ankle joint 57. Similarly, each arm 54 isprovided with a shoulder joint 59, an elbow joint 60, and a wrist joint61 at its proximal portion adjacent to the body 53 (the portionconnected to the body 53), its middle portion, and its distal portion,respectively, which are operated by actuators, such as electric motors,and a hand 62 is connected to the distal portion of each arm 54 throughthe wrist joint 61. In this example, the joints 55, 56, and 57 of eachleg 2 are joints having, for example, 3 degrees of freedom, 1 degree offreedom, and 2 degrees of freedom, respectively, and the foot 58 has 6degrees of freedom relative to the body 53. Further, the shoulder joint59, the elbow joint 60, and the wrist joint 61 of each arm 54 areconstructed such that their degrees of freedom allow the hand 62 to havea degree of freedom of 6 degrees or more with respect to the body 53.

Although not shown, the body 53 of the robot 51 is provided with acontrol device 50 similar to that of the aforesaid first embodiment.Further, a head 63 is provided at the upper end of the body 53.

In this robot 51, in the state wherein the robot 51 is on its knees(particular posture state), as shown in FIG. 58 and FIG. 59, theportions of the foot 58 of each leg 52 and each knee joint 56 (morespecifically, the surface portion of a link (shank link) connecting theknee joint 56 and the ankle joint 57 at near the knee joint 56.Hereinafter referred to simply as the knee) and the hand 62 of each arm54 are ground contact portions. And, in the present embodiment, as shownin FIG. 59, the knee, which is a ground contact portion, is providedwith a floor reaction force sensor 90 (load sensor). The floor reactionforce sensor 90 is constructed of a main body (sensor part) 92 and asoft member (elastic member) 94, such as a sponge. The main body 92 isfixed to the knee (leg link), and the outside of the main body 92 iscovered with a soft member (elastic member) 94. To enhance the accuracyof the compliance control in the knee, it is desirable to shape thesurface (ground contact surface) of the soft member 94 into a roundconvex surface in addition to covering the knee with the soft member 94.This arrangement reduces the nonlinearity of the relationship between acorrective operation of a desired motion of the robot 51 and a floorreaction force, resulting in better control performance of thecompliance control. Incidentally, although not shown, the foot 58 andthe ankle joint 57 are connected through a floor reaction force sensor,such as a six-axis force sensor, and the compliance mechanism.Similarly, the hand 62 and the wrist joint 61 are connected through afloor reaction force sensor, such as a six-axis force sensor, and thecompliance mechanism, which are not shown. The connecting constructionsmay be ones that am publicly known.

The floor reaction force sensor 90 of the ground contact portion of theknee joint may be any one of the following.

-   1) Sensor that detects only the translational force in the direction    perpendicular to the surface of a ground contact portion that comes    in contact with a floor (ground contact surface).-   2) Sensor that detects not only the translational force in the    direction perpendicular to the surface of a ground contact portion    that comes in contact with a floor (ground contact surface) but also    the translational force in a direction other than the direction    perpendicular to the contact surface.-   3) Distributed pressure sensor-   4) Sensor that detects moments in addition to translational forces    (e.g., six-axis force sensor)

Alternatively, a displacement sensor for detecting the deformation(distortion) of the aforesaid soft member 94 may be used in place of onethat directly detects a load.

In the present embodiment, as the floor reaction force sensor 90, thesensor of the above 2) will be used. To further enhance the accuracy ofthe compliance control, the sensor of the above 3) or 4) may be used asthe floor reaction force sensor 90. Incidentally, as the structure ofthe knee, a floor reaction force sensor may be added to a protective padprovided with a spring (a pad for protecting the knee), as disclosed inJapanese Unexamined Patent Application Publication No. 2000-62760previously proposed by the present applicant.

In the present embodiment, the hierarchical structure as shown in FIG.61 is set for the robot 51 on its knees as described above.Specifically, the right foot 58, the left foot 58, the right knee, theleft knee, the right hand 62, and the left hand 62 as the ground contactportions are associated with a first node, a second node, a third node,a fourth node, a fifth node, and a sixth node, respectively, which areleaf nodes. Further, a 12th node having the first node and the secondnode as child nodes, a 34th node having the third node and the fourthnode as child nodes, a 56th node having the fifth node and the sixthnode as child nodes, and a 1234th node having the 12th node and the 34thnode as child nodes have been set as intermediate nodes. Further, a123456th node having the 1234th node and the 56th node as child nodeshas been set as the root node. Further, according to the policy similarto that in the aforesaid first embodiment and the like, a desired floorreaction force central point Qn(n=1, 2, 3, 4, 5, 6, 12, 34, 56, 1234,123456) is associated with each node, as shown in the figure.Incidentally, the desired floor reaction force central point Q123456 ofthe 123456th node, which is the root node, is identical to a desiredtotal floor reaction force central point P.

In the present embodiment, the control device 50 is supposed to have afunctional construction similar to that shown in the aforesaid FIG. 2.

In this case, the desired ground contact portion trajectory of a desiredmotion In a desired gal output by a gal generating device 100 in thepresent embodiment is constructed of the desired position/posturetrajectory of each hand 62, the desired position/posture trajectory ofeach foot 58, and the desired position trajectory of each knee. In thiscase, the gait generating device 100 preferentially determines desiredfoot positions/postures (desired first and second ground contact portionpositions/postures), desired hand positions/postures (desired fifth andsixth ground contact portion positions/postures), and desired kneepositions (desired third and fourth ground contact portion positions) sothat the feet 58, the hands 62, and the knees come in contact with theground on a supposed floor surface as required for the gait, thendetermines a desired ZMP (desired total floor reaction force centralpoint) in a supporting polygon, which is a minimum convex polygon thatincludes the desired ground contact point (or a desired ground contactline or a desired ground contact surface) of each ground contactportion, and then desired body position/posture are determined by usinga dynamic model of the robot 51 such that the desired footpositions/postures, the desired hand positions/postures, the desiredknee positions, and the desired ZMP are satisfied.

If the head 63 can be moved with respect to the body 53, then thedesired position/posture trajectory of the head 63 is included in adesired motion.

In the present embodiment, the arithmetic processing of the robotgeometric model (inverse kinematics calculator) 110 shown in FIG. 2differs from that of the first to the third embodiments, as will bediscussed later.

Further, in the present embodiment, a part of the processing techniqueof a hierarchical compliance operation determiner 114 differs from theone explained in the first to the third embodiments described above.FIG. 62 shows the functional construction of the hierarchical complianceoperation determiner 114 of the present embodiment. In the processing ofthis functional construction, major aspects that are different from thefirst to the third embodiments will be explained. A compensating totalfloor reaction force moment distributor 114 a determines and outputs notonly the node compensating floor reaction force moments of theintermediate nodes and the root node, but also a node compensating floorreaction force moment Mn_dmd of an n-th node (n=1, 2, 5, 6) of the leafnodes, that is, the required floor reaction force moment (the requiredmoment for restoring the posture) about a desired n-th ground contactportion floor reaction force central point (n=1, 2, 5, 6). This isbecause, in the robot 51, moments can be generated about the desiredfloor reaction force central points of the feet 58 and the hands 62 asthe ground contact portions. The technique for determining the nodecompensating floor reaction force moment Mn_dmd, including the requiredmoments, may be the same technique explained in the first and the secondembodiments described above. Further, in the present embodiment, thecompensating total floor reaction force moment distributor 114 adetermines and outputs a corrected desired n-th ground contact portionfloor reaction force moment Mn_refmdfd(n=1, 2, 5, 6) about a desiredn-th ground contact portion floor reaction force central point (n=1, 2,5, 6), which is corrected by generating a node compensating floorreaction force moment about the desired floor reaction force centralpoint of each corresponding node, together with a translational forcecomponent Fn_refmdfd(n=1, 2, 3, 4, 5, 6) of a corrected desired groundcontact portion floor reaction force. These are used by a deformationcompensation amount calculator 114 n to calculate a deformationcompensation amount.

A compensating angle determiner (the part enclosed by the virtual linein FIG. 62) in the present embodiment determines and outputs not onlythe compensating angles of the intermediate nodes and the root node butalso the node compensating angle θn of an n-th node (n=1, 2, 5, 6) amongthe leaf nodes. A θn determiner that determines these n-th nodecompensating angles θn(n=1, 2, 5, 6) determines an n-th nodecompensating angle θn(n=1, 2, 5, 6) by the same processing as that ofthe foot compensating angle determiner disclosed in Japanese UnexaminedPatent Application Publication No. H10-277969. Incidentally, thetechnique for determining the compensating angles θn(n=12, 34, 56, 1234,123456) of the intermediate nodes and the root node other than the abovenodes may be the same as the technique explained in the first and thesecond embodiments described above.

Further, a corrected desired ground contact portion position/posturecalculator 114 g in the present embodiment combines node rotationalmanipulations on the basis of each n-th node compensating angle θn(n=12,34, 56, 1234, 123456) by the technique explained in the first and thesecond embodiments describe above so as to correct a desired groundcontact portion position, and it further corrects the desired posture ofan n-th ground contact portion (n=1, 2, 5, 6), thereby determining thecorrected desired ground contact portion position/posture of each groundcontact portion.

Further, according to the present embodiment, a floor configurationestimator 130 determines an estimated n-th floor height errorZfn_estm(n=1, 2, 3, 4, 5, 6) by the same technique as the techniqueexplained in the aforesaid third embodiment and also estimates a groundcontact portion floor inclination error of an n-th ground contactportion θfn(n=1, 2, 5, 6). The estimation of the ground contact portionfloor inclination error θfn is performed by the same processing as theprocessing for estimating foot floor inclinations by the floorconfiguration estimator disclosed in Japanese Unexamined PatentApplication Publication No. H10-277969.

The processing techniques of the functional construction of thehierarchical compliance operation determiner 114 of the presentembodiment other than the ones described above are the same as that inthe third embodiment.

Except for the difference aspects explained above, the arithmeticprocessing of the control device 50 in the present embodiment is thesame as that in the first to the third embodiments.

Next, the processing of the aforesaid robot geometric model 110 in thepresent embodiment will be explained in detail.

In the robot 51 of the present embodiment, the number of the degrees offreedom of the joints from the body 53 to the knees is limited to three(only 3 degrees of freedom in the hip joints 55); therefore, it isimpossible to change only the difference in height between the right andleft knees while strictly maintaining body position/posture, footposition/posture, the horizontal positions of the right and left knees,and the sum of the heights of the right and left knees at their desiredvalues. In other words, if an attempt is made to forcibly change onlythe height difference between the right and left knees, then twist orslippage would inconveniently take place between ground contact portions(namely, the knees and the feet) and a floor.

Similarly, it is impossible to change only the difference in heightbetween the right and left feet while strictly maintaining the bodyposition/posture, the positions of both knees, the horizontal positionsof both feet, and the sum of the heights of the right and left feet attheir desired values. In other words, if an attempt is made to forciblychange only the height difference between the right and left feet, thentwist or slippage would inconveniently take place between ground contactportions (namely, the knees and the feet) and a floor.

Similarly, it is impossible to change only the sum of the heights of theright and left knees while strictly maintaining the bodyposition/posture, foot positions/postures, the horizontal positions ofthe right and left knees, and the height difference between the rightand left knees at their desired values. In other words, if an attempt ismade to forcibly change only the sum of the heights of the right andleft knees, then the body position would be dislocated from a desiredposition even though twist or slippage may not take place between groundcontact portions (namely, the knees and the feet) and a floor. As aresult, the position of the total center-of-gravity of the robot 51 andan inertial force will deviate from desired values, leading todeteriorated stability of the robot 51.

This means that, in any case, the corrected desired ground contactportion position/posture with deformation compensation and desired bodyposition/posture that have been determined by the hierarchicalcompliance operation determiner 114 cannot be strictly satisfied at thesame time, so that twist or slippage takes place between the feet 58 orthe knees of the robot 51 and the floor or the total center-of-gravityof the robot 51 and an inertial force deviate from their desired values,possibly resulting in deterioration of the stability of the robot 51when the robot 51 is in the posture state shown in FIG. 58 and FIG. 59.Incidentally, the hands 62 among the ground contact portions have adegree of freedom of 6 degrees or more of freedom with respect to thebody 53; therefore, no twist or slippage will occur between the hands 62and the floor unless the robot 51 is made to take a posture in which itcomes in contact with the floor through portions other than the hands 62of the arms 54.

Hence, in the present embodiment, the posture or the position/posture ofthe body 53 are corrected on the basis mainly of a change in the heightdifference between the right and left knees while restraining, as muchas possible, the occurrence of twist or slippage between the knees andthe feet 58 among the ground contact portions of the robot 51 and thefloor and also restraining, as much as possible, a change in theposition of the center-of-gravity (especially the horizontal position)of the body 53.

FIGS. 63( a) and (b) show examples of the operations for correcting theposture of the body 53. FIG. 63( c) will be discussed later. These FIGS.63( a), (b), and (c) omit showing the arms 54 and the head 63.

One of the operations for correcting the posture of the body 53 is theoperation in which, from the state wherein the robot 51 is on its kneesas shown in FIG. 63( a), the body 53 is rotated (rotated as indicated byan arrow y1) by using, as the axis of rotation, the trunk axis passingapproximately the center-of-gravity G of the body 53, as shown in FIG.63( b), on the basis of a change in the height difference between theright and left knees caused by compliance control.

Further, in the posture in which the robot 51 is on its knees, as shownin FIG. 63, a change in the height difference between the right and leftfeet 58 and 58 causes the height difference between the right and leftknees to change by about half the change in the height differencebetween the right and left feet 58 and 58. Hence, the same operation asthat of correcting the posture of the body 53 on the basis of a changein the height difference between the right and left knees is performedby only a half amount, as compared with the case of a change in theheight difference between the right and left feet, on the basis of achange in the height difference between the right and left feet 58 and58 caused by the compliance control.

Further, when correcting the posture of the body 53 as described above,the position and the posture of the body 53 are corrected on the basisof a change in the sum of the heights of the right and left knees causedby the compliance control while restraining the occurrence of twist orslippage between ground contact portions (the knees and the feet 58) andthe floor as much as possible, and also restraining, as much aspossible, a change in the position (especially the horizontal position)of the center-of-gravity G of the body 53 or in the inclination of asegment connecting the center-of-gravity G and a total floor reactionforce central point.

FIG. 64 visually shows an operation for correcting the position and theposture of the body 53 on the basis of changes in the sum of the heightsof the right and left knees. Specifically, from the posture of the robot51 indicated by the dashed lines, as both knees are operated to movedown by the compliance control to the posture of the robot 51 indicatedby the solid lines, the bottom end portion (or the waist) of the body 53is shifted forward, as indicated by an arrow y3, and the inclination ofthe body 53 is shifted backward (in the direction in which the body 53rises), as indicated by an arrow y2. In other words, the body 53 istilted backward while maintaining the position of the center-of-gravityG of the body 53 (or the position of a predetermined representativepoint of the body 53), especially the horizontal position thereof.Alternatively, the body 53 is tilted backward while maintaining theinclination of the segment connecting the center-of-gravity G and thedesired total floor reaction force central point P. Further, as anoperation for raising both knees is performed by the compliance control,the bottom end portion (or the waist) of the body 53 is shiftedbackward, inversely from the above, to shift the inclination of the bodytoward the front. In other words, the body 53 is tilted forward whilemaintaining the position of the center-of-gravity G of the body (or theposition of the predetermined representative point of the body),especially its horizontal position. Alternatively, the body 53 is tiltedforward while maintaining the inclination of the segment connecting thecenter-of-gravity G and the desired total floor reaction force centralpoint P. Incidentally, Q1″ and Q3″ in FIG. 64 denote the desired floorreaction force central point of the foot 58 and the desired floorreaction force central point of the knee, respectively, after theposition/posture of the body 53 has been corrected as described above.In this example, Q1″ is identical to a desired floor reaction forcecentral point Q1 of the foot 58 before the correction.

The above is the overview of the operation for correcting the postureand the position of the body 53.

The processing function of the robot geometric model (inverse kinematicscalculator) 110 in the present embodiment is shown by the block diagramof FIG. 65.

In the inverse kinematics calculator 110, first, based on correcteddesired n-th ground contact portion position/posture with deformationcompensation (n=1, 2, . . . , 6) and the desired ground contact portionpositions/postures of the feet 58 and the knees determined by the gaitgenerating device 100, the correction amount of the height differencebetween the right and left knees Zkneediffmdfd and the correction amountof the sum of the heights of the right and left knees Zkneesummdfd forthe desired positions of the right and left knees determined by the gaitgenerating device 100, and the correction amount of the heightdifference between the right and left feet 58 and 58 Zfootdiffmdfd forthe desired positions of the right and left feet determined by the gaitgenerating device 100 are determined.

Specifically, the correction amount of the height difference between theright and left knees Zkneediffmdfd, the correction amount of the sum ofthe heights of the right and left knees Zkneesummdfd, and the correctionamount of the height difference between the right and left feetZfootdiffmdfd are determined according to the following expressions 47,48, and 49.Zkneediffmdfd=Corrected desired fourth ground contact portion positionwith deformation compensation−Desired fourth ground contact portionposition−Corrected desired third ground contact portion position withdeformation compensation+Desired third ground contact portionposition  Expression 47Zkneesummdfd=Corrected desired fourth ground contact portion positionwith deformation compensation−Desired fourth ground contact portionposition+Corrected desired third ground contact portion position withdeformation compensation−Desired third ground contact portionposition  Expression 48Zfootdiffmdfd=Corrected desired fourth ground contact portion positionwith deformation compensation−Desired fourth ground contact portionposition−Corrected desired third ground contact portion position withdeformation compensation+Desired third ground contact portionposition  Expression 49

More precisely, “portion position” in expressions 47 to 49 is the heightcomponent (the component in the vertical direction) of the portionposition.

Subsequently, the body position/posture correction amount are determinedon the basis of the correction amount of the height difference betweenthe right and left knees Zkneediffmdfd, the correction amount of the sumof the heights of the right and left knees Zkneesummdfd, and thecorrection amount of the height difference between the right and leftfeet Zfootdiffmdfd.

The body position/posture correction amount is composed of a bodyposition correction amount for knee height difference Xbkneediffmdfd, abody posture correction amount for knee height differenceθbkneediffmdfd, a body position correction amount for knee height sumXbkneesummdfd, a body posture correction amount for knee height sumθbkneesummdfd, a body position correction amount for foot heightdifference Xbfootdiffmdfd, and a body posture correction amount for footheight difference θbfootdiffmdfd. Specifically, these values aredetermined as follows.

First, based on the correction amount of the height difference betweenthe right and left knees Zkneediffmdfd, the body position correctionamount for knee height difference Xbkneediffmdfd and the body posturecorrection amount for knee height difference θbkneediffmdfd forpreventing the correction amount of the height difference between theright and left knees Zkneediffmdfd from causing twist or slippagebetween ground contact portions (the knees and the feet 58) and a floorare determined by geometric calculation on the basis of the correctionamount of the height difference between the right and left kneesZkneediffmdfd and the desired posture (desired motion) of the robot 51at that instant (current time).

To be more specific, for example, Xbkneediffmdfd and θbkneediffmdfd aredetermined according to the following expression 50, where Kxkneediffand Kthkneediff denote proportional coefficients based on desiredpostures of the robot 51.Xbkneediffmdfd=Kxkneediff*Zkneediffmdfdθbkneediffmdfd=Kthkneediff*Zkneediffmdfd  Expression 50

As an alternative, the relationship between the correction amount of theheight difference between the right and left knees (or a 34th nodecompensating angle θ34) and the correction amounts of the bodyposition/posture may be determined beforehand for some representativedesired postures of the robot 51, and it may be stored as a map orfunction so as to determine Xbkneediffmdfd and θbkneediffmdfd on thebasis of the map or function and the correction amount of the heightdifference between the right and left knees Zkneediffmdfd. Incidentally,in the present embodiment, the body posture is corrected by rotating thebody 53 about its trunk axis on the basis of the correction amount ofthe height difference between the right and left knees, so that the bodyposition correction amount for knee height difference Xbkneediffmdfd maybe zero.

Next, in the same manner as described above, based on the correctionamount of the height difference between the right and left feet 58 and58 Zfootdiffmdfd, the body position correction amount for foot heightdifference Xbfootdiffmdfd and the body posture correction amount forfoot height difference θbfootdiffmdfd for preventing the correctionamount of the height difference between the right and left feet 58 and58 Zfootdiffmdfd from causing twist or slippage between ground contactportions (the knees and the feet 58) and a floor are determined bygeometric calculation on the basis of the correction amount of theheight difference between the right and left feet Zfootdiffmdfd and thedesired posture (desired motion) of the robot 51 at that instant(current time).

To be more specific, for example, Xbfootdiffmdfd and θbfootdiffmdfd aredetermined according to the following expression 51, where Kxfootdiffand Kthfootdiff denote proportional coefficients based on desiredpostures of the robot 51. As described above, the influence of theheight difference of the feet 58 is about half the influence of theheight difference of the knees, so that Kxfootdiff and Kthfootdiff areone half of Kxkneediff and Kthkneediff.Xbfootdiffmdfd=Kxfootdiff*Zfootdiffmdfdθbfootdiffmdfd=Kthfootdiff*Zfootdiffmdfd  Expression 51

Incidentally, in the present embodiment, the body posture is correctedby rotating the body 53 about its trunk axis on the basis of thecorrection amount of the height difference between the right and leftfeet 58 and 58, so that the body position correction amount for footheight difference Xbfootdiffmdfd may be zero.

Subsequently, in the same manner as described above, based on thecorrection amount of the sum of the heights of the right and left kneesZsummdfd, the body position correction amount for knee height sumXbsummdfd and the body posture correction amount for knee height sumθbsummdfd for preventing the correction amount of the sum of the heightsof the right and left knees Zsummdfd from causing twist or slippagebetween ground contact portions (the knees and the feet) and a floor aredetermined by geometric calculation on the basis of the correctionamount of the sum of the heights of the right and left knees Zsummdfdand the desired posture of the robot 51 at that instant (current time).

Alternatively, the relationship between the correction amount of the sumof the heights of the right and left knees and the correction amount ofthe body posture may be determined beforehand for some representativedesired postures, and it may be stored as a map or function so as todetermine Xbsummdfd and θbsummdfd on the basis of the map or functionand the correction amount of the sum of the heights of the right andleft knees Zsummdfd.

Alternatively, the correction amounts of body position/posture may bedetermined as follows. The height difference between the right and leftknees is converted into the 34th node compensating angle θ34 that causesthe difference, the sum of the heights of the right and left knees isconverted into the 1234th node compensating angle θ1234, and the heightdifference between the right and left feet is converted into the 12thnode compensating angle θ12 that causes the difference. Then, based onthese converted compensating angles, the body position/posturecorrection amounts may be determined by geometric calculation.Alternatively, the relationship between the converted compensatingangles and body position/posture correction amounts may be determinedbeforehand for some representative desired postures of the robot 51, andit may be stored as a map or function so as to determine bodyposition/posture correction amounts on the basis of the map or functionand the converted compensating angles.

Subsequently, the desired body position/posture are moved (rotationalmovement and parallel movement) by the body position/posture correctionamounts to determine desired body position/posture with correction.Specifically, the desired body position/posture are rotationally movedabout a trunk axis (or a predetermined axis of rotation (the axis ofrotation substantially in a vertical plane)) by the sum of the bodyposture correction amount for knee height difference and the bodyposture correction amount for foot height difference(θbkneediffmdfd+θbfootdiffmdfd), and this is further rotationally movedabout a lateral axis by the body posture correction amount for the sumof knee heights θbfootsummdfd, then this is further moved in parallel inthe longitudinal direction of the robot 51 by the sum of the bodyposition correction amount for knee height difference, the body positioncorrection amount for the sum of knee heights, and the body positioncorrection amount for foot height difference(Xbkneediffmdfd+Xbkneesummdfd+Xbfootdiffmdfd) thereby to determinedesired body position/posture with twist correction.

Subsequently, based on the desired body position/posture with twistcorrection and desired n-th ground contact portion position/posture withdeformation compensation (n=1, 2, . . . , 6), a joint displacementcommand of the robot 51 is determined.

Thus, the processing of the inverse kinematics calculator 110 is carriedout as described above.

In other words, according to the sixth embodiment, if the degree offreedom is geometrically insufficient for the operation of correctingthe ground contact portion positions/postures of the robot 51, then thehierarchical compliance operation corrects not only the desired groundcontact portion positions/postures but also the desired bodypositions/postures such that a point of the body position (therepresentative point of the body) or the point (e.g., the overallcenter-of-gravity) of the weighted average position of a plurality ofportions, including the body (the weight in this case is desirably themass ratio of each portion), especially the horizontal position of thepoint is maintained, or the angle of a line that connects the point anda desired total floor reaction force central point (desired ZMP) ismaintained at the angle in a desired gait.

Further, in other words, the sixth embodiment is provide with a meansthat gives priority to the controllability of an actual floor reactionforce moment generated about a desired total floor reaction forcecentral point (desired ZMP) and determines at least one of acompensating height and a compensating angle as the manipulated variableof the relative height or the inclination angle of a predeterminedground contact portion A (a knee in this case) or the manipulatedvariable of the relative height or the inclination angle of a nodehaving a predetermined ground contact portion A as a descendant node inorder to enhance the stability of the entire posture control of therobot 51, a means for determining the correction amount of at least oneof the posture and the position of the body while maintaining theposition of the body or the aforesaid weighted average position of aplurality of portions, including the body, at approximately the positionin a desired gait on the basis of at least one of the aforesaidcompensating height and compensating angle, and an inverse kinematicscalculating means for determining a joint displacement on the basis ofat least one of the position and the posture of a predetermined groundcontact portion B (foot in this case) except for the aforesaidpredetermined ground contact portion A and the body position/posturecorrected by the aforesaid correction amount.

Incidentally, in the sixth embodiment, instead of rotating a bodyposture about the trunk axis of the body 53 of the robot 51, the body 53may be rotated using the vertical axis as the axis of rotation and usingthe waist (the bottom end portion of the body 53) of the robot 51 as thecenter, as shown in FIG. 64( c). In this case, however, the deviation ofthe center-of-gravity G of the body 53 in the lateral direction wouldincrease. Alternatively, the body posture may be rotated using an axislocated midway between the trunk axis of the body 53 and the verticalaxis as the axis of rotation. Alternatively, the position and theposture of the body 53 may be simultaneously corrected on the basis ofthe correction amount of the height difference between both knees or thecorrection amount of the height difference between both feet 58 and 58.Further, instead of maintaining the position of the center-of-gravity Gof the body 53 unchanged, the position/posture of the body 53 may becorrected such that the overall center-of-gravity position of the robot51 or the position of the representative point of the body 53 remainunchanged.

In any case, if the degree of freedom is geometrically insufficient inthe operation for correcting the position/posture of a ground contactportion of the robot 51 by the compliance operation or the like, atleast one of the position and the posture of the body 53 may becorrected from the position/posture of a desired gait so as to restrainthe occurrence of slippage, such as twist, of a ground contact portion.Further, instead of correcting at least one of the position and theposture of the body 53, at least one of the position and the posture ofa predetermined portion other than the body 53 may be corrected.

SEVENTH EMBODIMENT

An explanation will now be given about a control device for a mobilerobot according to a seventh embodiment of the present invention.

In the sixth embodiment, the position/posture of the body has beencorrected in order to prevent slippage, such as twist, of a groundcontact portion and to minimize the deviations of the position of theoverall center-of-gravity of the robot 51 and an inertial force. In thepresent embodiment (the seventh embodiment), slippage, such as twist, ofa ground contact portion has been allowed to a certain extent, and ajoint displacement (the displacement between a hip joint 55 and a kneejoint 56) or the position/posture of a ground contact portion have beenpreferentially corrected so as to restrain the deviations of theposition of the overall center-of-gravity of the robot 51 and aninertial force.

More specifically, in the present embodiment, an inverse kinematicscalculator 110 carries out the processing shown by the block diagram ofFIG. 66 in place of the processing of FIG. 65 explained in the sixthembodiment. Except for this difference, the present embodiment is thesame as the sixth embodiment.

The inverse kinematics calculator 110 of the present embodiment will beexplained with reference to FIG. 66. First, based on corrected desiredn-th ground contact portion position/posture with deformationcompensation (n=1, 2, . . . , 6) (specifically, the position out of theposition/posture) and desired n-th ground contact portionposition/posture (n=1, 2, . . . , 6) (specifically, the position out ofthe position/posture), a correction amount of the height differencebetween the right and left knees Zkneediffmdfd, a correction amount ofthe sum of the heights of the right and left knees Zkneesummdfd, and acorrection amount of the height difference between the right and leftfeet Zfootdiffmdfd are determined according to the aforesaid expression47, expression 48, and expression 49, and further, the height differencebetween the right and left knees is converted into a 34th nodecompensating angle θ34 that causes the difference. Similarly, the sum ofthe heights of the right and left knees is converted into a 1234th nodecompensating angle θ1234 that causes the sum, and the height differencebetween the right and left feet is converted into a 12th nodecompensating angle θ12 that causes the difference.

Next, based on the converted node compensating angles, jointdisplacement correction amounts are determined. This is performed asfollows. First, a case is assumed where a joint displacement iscorrected while fixing the body position/posture of a desired gait andretaining the angle of a segment that connects a desired total floorreaction force central point (a desired 123456th node floor reactionforce central point (point P in FIG. 61)) and the overallcenter-of-gravity of the robot 51 (or the center-of-gravity of the bodyor the center-of-gravity of a plurality of portions, including the body53) unchanged. In this case, a j-th joint displacement correction amount(j=1, 2, . . . ,total number of joints) for the inclination angle(compensating angle) of an n-th node (n=12, 34, 1234) corresponding toeach of ground contact portions (a knee and foot 58) to become a unitperturbation amount is determined, and this is defined as a sensitivityLn_j of the j-th joint displacement relative to the n-th nodecompensating angle.

More generally, the j-th joint displacement correction amount (j=1, 2, .. . ,total number of joints) for the relative angle between the segmentconnecting a desired total floor reaction force central point (a desired123456th node floor reaction force central point) and an overallcenter-of-gravity of the robot 51 (or the center-of-gravity of the bodyor the center-of-gravity of a plurality of portions, including the body53) and the inclination angle of an n-th node (n=12, 34, 1234)corresponding to each of ground contact portions (a knee and the foot58) to become a unit perturbation amount is determined when a jointdisplacement of a desired gait has been corrected, and this is definedas a sensitivity Ln_j of the j-th joint displacement relative to then-th node compensating angle.

Specifically, if Ln_knee_r denotes the sensitivity of a right knee jointdisplacement relative to an n-th node compensating angle (n=12, 34,1234), Ln_knee_l denotes the sensitivity of a left knee jointdisplacement relative to the n-th node compensating angle, Ln_hip_rdenotes the sensitivity of a right hip pitch joint displacement relativeto the n-th node compensating angle, and Ln_hip_l denotes thesensitivity of a left hip pitch joint displacement relative to the n-thnode compensating angle, then each sensitivity is set according to thefollowing expression 52 in the posture of the robot 51 on its kneesshown in FIG. 58 and FIG. 59.L1234_hip_r=0, L1234_hip_l=0, L1234_knee_r=1, L1234_knee_l=1,L12_hip_r=0, L12_hip_l=1L12_knee_r=a12, L12_knee_l=−a12, L34_hip_r=a34,L34_hip_l=−a34, L34_knee_r=0, L34_knee_l=0,  Expression 52where a12 and a34 denote predetermined constants.

Subsequently, joint displacement correction amounts are determinedaccording to expressions 53 to 56 given below, where θknee_r denotes aright knee joint displacement correction amount, θknee_l denotes a leftknee joint displacement correction amount, θhip_r denotes a right hipjoint displacement correction amount (more specifically, the jointdisplacement correction amount in the pitch direction of the right hipjoint), and θhip_l denotes a left hip joint displacement correctionamount (more specifically, the joint displacement correction amount inthe pitch direction of the left hip joint).θknee_(—) r=L1234_knee_(—) r*θ1234+L12_knee_(—) r*θ12+L34_knee_(—)r*θ34  Expression 53θknee_(—) l=L1234_knee_(—) l*θ1234+L12_knee_(—) l*θ12+L34_knee_(—)l*θ34  Expression 54θhip_(—) r=L1234_hip_(—) r*θ1234+L12_hip_(—) r*θ12+L34_hip_(—)r*θ34  Expression 55θhip_(—) l=L1234_hip_(—) l*θ1234+L12_hip_(—) l*θ12+L34_hip_(—)l*θ34  Expression 56

Other joint displacement correction amounts are determined in the samemanner; however, it will be omitted to simplify the explanation.

For the 1234th compensating angle θ1234, the displacements (angles) ofboth knee joints 56 are corrected by the same amount as θ1234, as shownin FIG. 67. Further, for the 12th compensating angle θ12, thedisplacement (angle) of the right knee joint 56 is corrected inproportion to θ12 and also the displacement (angle) of the left kneejoint 56 is corrected at the same time by the amount obtained bymultiplying the correction amount of the displacement of the right kneejoint 56 by −1. In other words, the displacement (angle) of the rightknee joint 56 is corrected, as shown in FIG. 67, and the displacement(angle) of the left knee joint 56 is corrected in reverse therefrom.Further, for the 34th compensating angle θ34, the displacement (angle)of the right hip joint 55 in the pitch direction is corrected inproportion to θ34 and also the displacement (angle) of the left hipjoint 55 in the pitch direction is corrected at the same time by theamount obtained by multiplying the correction amount of the displacementof the right hip joint 55 in the pitch direction by −1. In other words,the displacement (angle) of the right hip joint 55 in the pitchdirection is corrected, as shown in FIG. 68, and the displacement(angle) of the left hip joint 55 in the pitch direction is corrected inreverse therefrom. Incidentally, Q1 and Q1″ in FIG. 67 denote a desiredfloor reaction force central point of the foot 58 before the correctionsof the joint displacements described above and a desired floor reactionforce central point of the foot 58 after the corrections, respectively.Further, Q3 and Q3″ in FIG. 68 denote a desired floor reaction forcecentral point of a knee before the corrections of the jointdisplacements described above and a desired floor reaction force centralpoint of the knee after the corrections, respectively.

By correcting joint displacements as described above, the relative anglebetween the segment connecting a desired total floor reaction forcecentral point (a desired 123456th node floor reaction force centralpoint) and the overall center-of-gravity of the robot 51 (or thecenter-of-gravity of the body or the center-of-gravity of a plurality ofportions, including the body 53) and the inclination angle of an n-thnode (n=12, 34, 1234) corresponding to ground contact portions (a kneeand the foot 58) is changed by the aforesaid converted node compensatingangle θ12, θ23, and θ1234. Thus, an actual floor reaction force isfaithfully controlled, leading to improved posture stability and groundcontact properties of the robot 51.

Instead of directly correcting joint displacement commands (desiredjoint displacements) as described above, preferentially correcteddesired ground contact portion positions/postures, which are thecorrected desired positions/postures of the ground contact portions(specifically, the feet 58 and the hands 62) whose positions/posturesare to be preferentially corrected to cause approximately the samedesired joint displacements may be determined, and then, based on thedetermined preferentially corrected desired ground contact portionpositions/postures, joint displacement commands may be determined byinverse kinematics calculation.

Because of insufficient degrees of freedom of joints, it is impossibleto determine joint displacements by the inverse kinematics calculationto satisfy desired body position/posture and all corrected desiredground contact portion positions/postures; therefore, when carrying outthe inverse kinematics calculation, some corrected desired groundcontact portion positions/postures out of all corrected desired groundcontact portion positions/postures are used. The some corrected desiredground contact portion positions/postures are referred to as theaforesaid preferentially corrected desired ground contact portionpositions/postures.

For example, in the situation shown in FIG. 67, for the 1234th nodecompensating angle θ1234, the position obtained by rotationally moving adesired foot position about a knee may be determined as thepreferentially corrected desired foot position.

Incidentally, in the sixth embodiment and the seventh embodiment, as anexample, the compliance operation in the situation wherein the robot 51is on its knees has been explained. The following will describe theadaptation to a compliance operation in a situation wherein the robot issitting on a chair. This will be explained as an eighth embodiment.

EIGHTH EMBODIMENT

FIG. 69 shows the construction of an essential section of a robotaccording to the present embodiment. This robot 71 is provided withfloor reaction force sensors 73 and 73 for detecting floor reactionforces (load sensors, such as six-axis force sensors) at the right andleft, respectively, of the base end surface of buttocks 72. Instead ofproviding the floor reaction force sensors 73 and 73 at the right andleft, a single floor reaction force sensor that detects the resultantforce of the forces applied to the right and left of the base endsurface of the buttocks 72 may be provided.

In this case, the outsides of the floor reaction force sensors 73 and 73are covered with a soft member (elastic member) 74, such as a sponge, asillustrated. To enhance the accuracy of the compliance control in theground contact portions of the buttocks 72, it is desirable to shape thesurface (ground contact surface) of the soft member (elastic member) 74into a round convex surface in addition to covering them with the softmember. It is desirable to provide the member 74 such that thehorizontal positions of the detection positions of the floor reactionforce sensors 73 and 73 (the positions of the sensor main bodies) arealigned with the horizontal position of the apex of the convex surfaceof the aforesaid member 74 especially when the robot 71 is sitting on achair through the intermediary of its buttocks 72. This arrangementreduces the nonlinearity of the relationship between a correctiveoperation of the position/posture of a ground contact portion of therobot 71 and a floor reaction force, resulting in better controlperformance of the compliance control of the robot 71.

In the robot 71, legs (link mechanisms) 52 and 52 are providedextendedly from the right and left sides of the buttocks 72. Thestructures of the legs 52 and 52, including their joints, are the sameas those of, for example, the aforesaid sixth embodiment. Hence, thesame reference marks as those related to the legs 52 and 52 in the sixthembodiment will be used, and the explanations thereof will be omitted.However, in the present embodiment, the knees of the legs 52 may not beprovided with floor reaction force sensors.

Further, as shown in the figure, a torso (body) 77 is provided on theupper side of the buttocks 72, and arms 79 and 79 are providedextendedly from both sides of the upper portion of the torso 77 throughthe intermediary of shoulder joints 78 and 78. Incidentally, the arms 79may have the same structure as that in the robot 51 of the aforesaidsixth embodiment. Further, the torso 77 is connected with the buttocks72 through the intermediary of a joint 80. In this case, the joint 80 isconstructed of a torso turning joint 80 a for turning the torso 77 inthe yaw direction relative to the buttocks 72, and a torso flexing joint80 b for tilting the torso 77 in the longitudinal and lateral directionsrelative to the buttocks 72. The joints provided in the robot 71 asdescribed above are operated by actuators, which are not shown. Althoughnot shown, a control device 50, which is similar to that in theaforesaid first embodiment and the like, is installed in the buttocks 72or the torso 77.

In the present embodiment, the hierarchical structure may be set, asshown in FIG. 70, for the robot 71 sitting on a chair or the likethrough the intermediary of the buttocks 72. More specifically, a rightfoot 58, a left foot 58, the right portion of the base end surface ofthe buttocks 72 (the portion to which a right floor reaction forcesensor 73 is attached), and the left portion of the base end surface ofthe buttocks 72 (the portion to which a left floor reaction force sensor73 is attached) as ground contact portions are associated with a firstnode, a second node, a third node, and a fourth node, respectively,which are leaf nodes. Further, a 12th node having the first node and thesecond node as child nodes and a 34th node having the third node and afourth node as child nodes have been set as intermediate nodes, and a1234th node having the 12th node and the 34th node as child nodes hasbeen set as a root node. Further, according to the policy similar tothat in the aforesaid first embodiment and the like, the nodes areassociated with desired floor reaction force central points Qn(n=1, 2,3, 4, 12, 34, 1234), as illustrated. Incidentally, a desired floorreaction force central point Q1234 of the 1234th node, which is the rootnode, is identical to a desired total floor reaction force central pointP.

In the present embodiment, the control device 50 is supposed to have afunctional construction similar to that shown in the aforesaid FIG. 2.

In this case, a desired ground contact portion trajectory of a desiredmotion in a desired gait output by a gait generating device 100 in thepresent embodiment is constructed of the desired position/posturetrajectory of each foot 58 and the desired position/posture trajectoryof the buttocks 72. The body position/posture trajectory of the desiredmotion means the position/posture trajectory of the torso 77.Incidentally, the desired motion includes the position/posturetrajectory of the distal portion of each arm 79. Further, the desiredtotal floor reaction force central point P is on a virtual plane in theair rather than on an actual floor surface.

In the present embodiment, a hierarchical compliance operationdeterminer 114 has the same functional components as those of theaforesaid sixth embodiment (refer to FIG. 62). However, in the presentembodiment, a compensating total floor reaction force moment distributor114 a determines and outputs the node compensating floor reaction forcemoments of the intermediate nodes and the root node in the hierarchicalstructure shown in FIG. 70 and the node compensating floor reactionforce moments of the leaf nodes corresponding to the feet 58. Further, acompensating angle determiner determines and outputs the nodecompensating angles of the intermediate nodes and the root node in thehierarchical structure shown in FIG. 70 and the node compensating anglesof the leaf nodes corresponding to the feet 58. In this case, the basictechniques for determining these node compensating floor reaction forcemoments and node compensating angles may be the same as the techniquesexplained in the first to the third embodiments or the sixth embodiment.Further, a floor configuration estimator determines estimated n-th floorheight errors Zfn_estm(n=1, 2, 3, 4) by the same technique as thetechnique explained in the aforesaid third embodiment, as in the case ofthe aforesaid sixth embodiment, and also estimates ground contactportion floor inclination errors θfn(n=1, 2) of the feet 58.

Further, a robot geometric model (inverse kinematics calculator), whichis a functional component of the control device 50, corrects thepositions/postures of the ground contact portion of the buttocks 72 andthe feet 58 (ground contact portions) to restrain the occurrence ofslippage, such as twist, of the ground contact portions and alsocorrects the position/posture of the body (torso) 77 basically on thebasis of the corrected desired ground contact portion positions/postureswith deformation compensation and the desired body position/posturementioned above by the same technique as that in the aforesaid sixthembodiment.

The processing of the control device 50 other than that explained abovemay be the same as that of the aforesaid sixth embodiment.

Several modifications related to the embodiments explained above willnow be explained.

An actual node floor reaction force that cannot be directly detected bya floor reaction force sensor may be estimated by an observer by usinganother actual ground contact portion floor reaction force directlydetected by a floor reaction force sensor, a detected value of anacceleration sensor, a detected value of a clinometer, and the like, orit may be estimated by a simplified algebraic relationship. Forinstance, the hierarchical compliance operation is a rotationalcompliance operation in which a ground contact portion is rotated abouta desired total floor reaction force central point; therefore, even whenthe compliance operation is performed, the vertical acceleration of theoverall center-of-gravity of the robot (or an acceleration component inthe direction of a segment that connects a desired total floor reactionforce central point and the overall center-of-gravity) substantiallyagrees with the vertical acceleration of the overall center-of-gravityin a desired gait (desired motion) of the robot (or an accelerationcomponent in the direction of a segment that connects a desired totalfloor reaction force central point and the overall center-of-gravity).Hence, the sum of all actual ground contact portion floor reaction forcevertical components substantially agrees with the value obtained bymultiplying the sum of the vertical acceleration of the overallcenter-of-gravity in the desired gait (desired motion) of the robot andthe gravitational acceleration by the total mass of the robot.

Thus, if an actual n-th node floor reaction force cannot be directlydetected, then first, the sum (hereinafter referred to as the actualnon-n-th node floor reaction force) of the actual floor reaction forces(actual ground contact portion floor reaction forces) of all leaf nodesthat do not have an n-th node as their ancestor node and that are notthe n-th node itself is determined.

Next, an estimated n-th node floor reaction force, which is an estimatedvalue of an actual n-th node floor reaction force, is determinedaccording to the following expression 57.Estimated n-th node floor reaction force=(Overall center-of-gravityacceleration of desired gait+Gravitational acceleration)*Totalmass−Actual n-th node floor reaction force  Expression 57

If the body or the like is provided with an acceleration sensor, thenthe center-of-gravity acceleration of the entire robot (hereinafterreferred to as an estimated overall center-of-gravity acceleration) maybe estimated on the basis of a detected value of the acceleration sensorand at least one of a desired gait and an actual joint displacement, andan estimated n-th node floor reaction force, which is the estimatedvalue of an actual n-th node floor reaction force, may be determinedaccording to the following expression 58.Estimated n-th node floor reaction force=(Estimated Overallcenter-of-gravity acceleration of+Gravitational acceleration)*Totalmass−Actual n-th node floor reaction forces  Expression 58

For example, in the aforesaid sixth embodiment, if no floor reactionforce sensors for detecting knee floor reaction forces are provided orfloor reaction force sensors for detecting knee floor reaction forcesfail, then an estimated 34th node floor reaction force is determinedaccording to the following expression 59.Estimated 34th node floor reaction force=(Overall center-of-gravityacceleration of desired gait+Gravitational acceleration)*Totalmass−Actual 34th node floor reaction forces=Overall center-of-gravityacceleration of desired gait*Total mass−(Actual 12th node floor reactionforce+Actual 56th node floor reaction force)  Expression 59

Further, the estimated 34th node floor reaction force is used in placeof an actual 34th node floor reaction force to determine the 1234th nodecompensating angle θ1234 by the compliance operation processing, and anestimated 1234th node relative floor height error Z1234rel_estm is alsoestimated by floor configuration estimation processing.

In determining the 34th node compensating angle θ34, it is assumed thatthe floor is as expected, and an estimated third node floor reactionforce, which is the estimated value of an actual third node floorreaction force, and an estimated fourth node floor reaction force, whichis the estimated value of an actual fourth node floor reaction force,are determined on the basis of the corrected desired third groundcontact portion position/posture with deformation compensation, thecorrected desired fourth ground contact portion position/posture withdeformation compensation, and an estimated 34th node floor reactionforce.

Further, the estimated third node floor reaction force and the estimatedfourth node floor reaction force are used in place of an actual thirdnode floor reaction force and an actual fourth node floor reaction forceto determine the 34th node compensating angle θ34 by the complianceoperation processing.

Instead of determining the corrected desired node floor reaction forcemoment about a desired node floor reaction force central point on thebasis of the required moment for restoring the posture of the robot(compensating total floor reaction force moment) so as to be equivalentto adding the required moment for restoration as described above, acorrected desired node floor reaction force central point obtained bycorrecting a desired node floor reaction force central point may bedetermined on the basis of the required moment for restoration(compensating total floor reaction force moment). In this case, adesired node floor reaction force moment about a desired node floorreaction force central point is not corrected, so that it remains to bezero.

Further, in order to compensate for a component that could not becontrolled within an n-th node floor reaction force (an expected ordetected control error component), the desired floor reaction force ofthe parent node of the n-th node may be corrected. More specifically,the value of the difference between an actual n-th node floor reactionforce and a desired n-th node floor reaction force in the last controlcycle or the value obtained by passing the aforesaid difference througha low-pass filter may be added to the desired floor reaction force ofthe n-th node.

In the processing of estimating a floor configuration, a tree structurethat is different from the tree structure for the compliance control maybe set. The tree structure may have two layers composed of a root nodeand leaf nodes. Further, the weights of nodes may be set to be differentfrom those for the compliance control. A node may be set to any weightas long as the weight of the node is zero in a period during which anode floor reaction force is zero (a period during which all groundcontact portions belonging to the node or the ground contact portioncorresponding to the node moves in the air).

Further, in the processing of estimating a floor configuration, eachcorrected node weight may be determined on the basis of the aforesaidcorrected desired node floor reaction force central point, and a vectorhaving the determined corrected node weight as an element may be used asthe weight for estimating the floor configuration.

When determining an estimated relative floor height error of each point(each ground contact portion) in the processing of estimating a floorconfiguration, instead of using an actual floor reaction force, amechanism compliance model, and the detected value of a posture sensor,a deformation amount detector for detecting the deformation amount of acompliance mechanism or the like may be provided so as to use a detectedvalue of the deformation amount detector and a detected value of aposture sensor.

If a floor configuration is to be not only relatively estimated butglobally estimated also (estimating a floor configuration on a globalcoordinate system), then the following may be carried out.

-   1) Estimating the global height of a floor in a case where the    height of the body of a robot on the global coordinate system    (hereinafter referred to as the global height) can be detected:

A detector that detects the global height of a predetermined portion,such as a body, of a robot (a body height estimator using anacceleration sensor (e.g., the estimator disclosed in PCT/JP03/05448 bythe present applicant) or an external sensor, such as a visual sensor)is used. The value of an estimated floor height error on the globalcoordinate system is determined on the basis of the estimated bodyglobal height, a joint displacement (an actual joint displacement or adesired joint displacement), the detected value of an actual bodyposture inclination, and the detected value of an actual floor reactionforce. Thus, the estimated floor height error of the root node will havea meaning as the weighted average value of the global estimated floorheight error of every ground contact portion.

-   2) Estimating the global height of a floor in a situation wherein    the global height of the body is unknown:

If the global value of a certain estimated n-th floor height error (theerror in the global coordinate system rather than the relative value ina group) is known, then the global values of the estimated floor heighterrors of all points (ground contact portions) are determined on thebasis of the above known global value and estimated relative floorheight errors. If the global value of the estimated n-th floor heighterror at the start point of a travel of a robot is known, then theglobal value of the floor height error of a ground contact portion thatnewly comes in contact with the ground will be sequentially determinedas the robot continues to travel; therefore, a map of floor heights on atravel trajectory of the robot can be created by storing the abovesequentially determined global values.

Further, in the processing of estimating a floor configuration, eachcorrected node weight (obtained by correcting the value of the weight ofeach node) may be determined on the basis of the aforesaid correcteddesired node floor reaction force central point, and the determinedcorrected node weight may be used as the weight for defining an actualn-th node relative floor height or the like in estimating the floorconfiguration. Further, in estimating a floor configuration, the weightused to define the actual n-th node relative floor height or the likedoes not necessarily have to be the same as the aforesaid weightdetermined by the desired floor reaction force distributor.

Supplementally, the weight for defining the actual n-th node relativefloor height or the like does not necessarily have to agree with theweight determined by the desired floor reaction force distributor;however, if they are to agree with each other (in this case, the desiredfloor reaction force central point of the root node will automaticallyagree with a desired total floor reaction force central point), then theinfluence of a floor configuration error can be canceled simply byadding an n-th node estimated floor inclination error to an n-th nodecompensating angle (n=1, 2, . . . ) for the compliance operation, thusmaking it possible to reduce the calculation volume for canceling theinfluence of the floor configuration error.

Furthermore, estimated floor configuration errors may be stored as themap information of a floor configuration, and a desired gait may begenerated on the basis of the stored map information of the floorconfiguration when a robot walks in the same place next time.

Incidentally, the arithmetic processing sequences in the block diagramsor the flowcharts showing the arithmetic processing functions in theembodiments described above may be subjected to equivalentmodifications.

Further, the present invention can be applied not only to a mobile robotbut also a wheel-type mobile body having an active suspension.

INDUSTRIAL APPLICABILITY

As described above, the present invention is useful as the one thatmakes it possible to provide a mobile body, such as a legged mobilerobot, that permits highly stable, smooth operations to be achieved byproperly controlling floor reaction forces.

1. A control device for a mobile body comprising: a desired motiondetermining means for determining a desired motion of the mobile body,having three or more ground contact portions connected through theintermediary of a plurality of joints such that mutual relative heightscan be manipulated; an actual posture inclination detecting means fordetecting or estimating the actual posture inclination of a base body ofthe mobile body, the operation of the mobile body being controlled tomake the motion of the mobile body follow a desired motion; and a nodeoperation controlling means that categorizes the ground contact portionsin a tree structure manner such that all of the ground contact portionsbecome leaf nodes, and an intermediate node exists between the leafnodes and a root node having all the leaf nodes as descendant nodes,that determines, on each of C-th nodes, which is a node having aplurality of ground contact portions as descendant nodes, the correctionamounts of desired relative heights of a plurality of ground contactportions belonging to the C-th node for each of the C-th nodes such thata posture inclination error is approximated to zero on the basis of atleast the posture inclination error, which is the difference between theactual posture inclination and a desired posture inclination of the basebody in the desired motion, determines, on the basis of a correctionamount obtained by combining the determined correction amounts of allthe C-th nodes, a corrected desired motion by adding at least a firstcorrection of the desired relative heights of the plurality of groundcontact portions belonging to each C-th node to the desired motion, andoperates the joints such that the corrected desired motion that has beendetermined is satisfied.
 2. The control device for a mobile bodyaccording to claim 1, wherein the ground contact portions are connectedto the base body of the mobile body in such a manner that they aremovable with respect to the base body, and when determining thecorrected desired motion, the node operation controlling meansdetermines the corrected desired motion by further adding to the desiredmotion a correction of a desired posture of the base body to restrainslippage of the ground contact portions belonging to the C-th node on afloor surface.
 3. The control device for a mobile body according toclaim 2, wherein the node operation controlling means determines thecorrected desired motion such that the horizontal position of theoverall center-of-gravity of the mobile body in the corrected desiredmotion or the horizontal position of a predetermined representativepoint of the base body of the mobile body is substantially the same asthe horizontal position in the desired motion.
 4. The control device fora mobile body according to claim 2, wherein the mobile body comprises aplurality of link mechanisms extended from its base body, and at leastone link mechanism out of the plurality of link mechanisms has a jointprovided at an intermediate portion between at least its distal portionand the end portion adjacent to the base body, the intermediate portionbeing the ground contact portion.
 5. The control device for a mobilebody according to claim 4, wherein the intermediate portion is providedwith an elastic member that resiliently deforms when it comes in contactwith the ground.
 6. The control device for a mobile body according toclaim 2, wherein the mobile body comprises a plurality of linkmechanisms which are extended from its base body and each of which hasone or more joints, and at least one link mechanism among the pluralityof link mechanisms and the base body are provided with the groundcontact portions.
 7. The control device for a mobile body according toclaim 6, wherein at least one ground contact portion of the base body isprovided with an elastic member that resiliently deforms when it comesin contact with the ground.
 8. The control device for a mobile bodyaccording to claim 1, the ground contact portions being connected to thebase body through the intermediary of connecting mechanisms such thatthey can be moved with respect to the base body of the mobile body,comprising: a means for determining a translational force component of adesired floor reaction force to be applied to ground contact portionsbelonging to the C-th node when the joint is operated such that thecorrected desired motion is satisfied, wherein the node operationcontrolling means comprises a means for estimating a deformation amountthat occurs at the connecting mechanisms between the base body and theground contact portions belonging to the C-th node and the groundcontact portions when the translational force component of the desiredfloor reaction force is applied to the ground contact portions belongingto the C-th node on the basis of the translational force components ofthe desired floor reaction forces of the ground contact portionsbelonging to at least the C-th node, and determines the correcteddesired motion by further adding a second correction of the desiredheights of the plurality of ground contact portions belonging to theC-th node, which is for canceling the estimated deformation amount, tothe desired motion.
 9. The control device for a mobile body according toclaim 1, comprising: a floor configuration estimating means forestimating a parameter that specifies the relative heights of the groundcontact surfaces of a plurality of ground contact portions belonging toat least the C-th node as the floor configuration parameter thatrepresents a floor configuration, wherein the node operation controllingmeans determines the corrected desired motion by further adding a thirdcorrection of the desired relative heights of a plurality of the groundcontact portions belonging to the C-th node on the basis of an estimatedvalue of a floor configuration parameter when determining the correcteddesired motion.
 10. The control device for a mobile body according toclaim 9, comprising: a floor reaction force detecting means fordetecting or estimating an actual floor reaction force that acts on eachof the ground contact portions, wherein the floor configurationestimating means performs estimation while sequentially updating thefloor configuration parameter on the basis of a past value of anestimated value of the floor configuration parameter, a differencebetween the corrected desired motion and the desired motion, thecorrected motion, at least either a detected value or an estimated valueof an actual joint displacement, which is a displacement amount of eachjoint of the mobile body, the actual posture inclination, and an actualfloor reaction force of each of the ground contact portions belonging toat least the C-th node.
 11. The control device for a mobile bodyaccording to claim 9, comprising: a floor reaction force detecting meansfor detecting or estimating an actual floor reaction force that acts oneach of the ground contact portions; and a means for sequentiallydetermining at least an actual node floor reaction force, which is anactual value of the node floor reaction force of each child node of theC-th node, from the actual floor reaction force of each ground contactportion belonging to the C-th node when A-th nodes, which are the nodesin the tree structure, the A-th nodes are the leaf nodes, then the floorreaction forces acting on the ground contact portions, which are theleaf nodes, are defined as the node floor reaction forces of the A-thnodes, and when the A-th nodes have child nodes, then the resultantforce of the node floor reaction forces of all the child nodes of theA-th nodes is defined as the node floor reaction force of the A-thnodes, wherein the floor configuration estimating means performsestimation while sequentially updating the floor configuration parameteron the basis of the past value of an estimated value of the floorconfiguration parameter, the difference between the corrected desiredmotion and the desired motion, the corrected motion, at least either adetected value or an estimated value of an actual joint displacement,which is a displacement amount of each joint of the mobile body, theactual posture inclination, and a relative relationship among the actualnode floor reaction forces of the child nodes of at least the C-th node.12. The control device for a mobile body according to claim 9, theground contact portions being connected to the base body through theintermediary of connecting mechanisms such that they are movable withrespect to the base body of the mobile body, comprising: a deformationamount detecting means for detecting or estimating the amounts ofdeformations that occur at the connecting mechanisms between the basebody and the ground contact portions and at the ground contact portions,wherein the floor configuration estimating means performs estimationwhile sequentially updating the floor configuration parameter on thebasis of the past value of an estimated value of the floor configurationparameter, a difference between the corrected desired motion and thedesired motion, the corrected motion, at least either a detected valueor an estimated value of an actual joint displacement, which is adisplacement amount of each joint of the mobile body, the actual postureinclination, and the deformation amount associated with each groundcontact portion belonging to at least the C-th node.
 13. The controldevice for a mobile body according to claim 12, comprising a floorreaction force detecting means for detecting or estimating an actualfloor reaction force acting on each of the ground contact portionsbelonging to the C-th node, wherein the deformation amount detectingmeans estimates the deformation amount on the basis of the actual floorreaction force.
 14. The control device for a mobile body according toclaim 9, wherein, each node in the tree structure has a set weight, whena B-th node, which is each node in the tree structure, is the leaf node,then the height of the ground contact surface of the ground contactportion, which is the leaf node, is defined as the height of the groundcontact surface of the B-th node, and when the B-th node has childnodes, then a weighted mean value, which uses the weight, of the heightsof the ground contact surfaces of all child nodes of the B-th node isdefined as the height of the ground contact surface of the B-th node,and when these definitions apply, the floor configuration parameterestimated by the floor configuration estimating means is a parameterthat specifies a relative relationship of the heights of the groundcontact surfaces of a plurality of child nodes of the C-th node.
 15. Thecontrol device for a mobile body according to claim 14, wherein when atleast one of the child nodes of the C-th node is about to float, thenthe floor configuration estimating means estimates the floorconfiguration parameter that specifies the relative relationship of theheights of the ground contact surfaces of the plurality of child nodeswhile retaining, at a fixed value, the relative height of the groundcontact surface of the child node that is about to float.
 16. Thecontrol device for a mobile body according to claim 9, wherein the floorconfiguration estimating means estimates the floor configurationparameter by using a low-pass filter to alleviate fluctuation in thefloor configuration parameter.
 17. The control device for a mobile bodyaccording to claim 9, wherein a weight for estimating a floorconfiguration is set on each node of the tree structure, wherein when apredetermined type of state amount is associated with each leaf node inthe tree structure, the state amount of each node having child nodes isdefined as a weighted mean value, which uses the weight, of the stateamounts of all child nodes of the node, a value obtained by subtractinga state amount of a parent node of each node from the state amount ofthe node is determined as a node relative state amount of the node onthe node except a root node, and zero is determined as a relative stateamount of the root node, is defined as the processing for relativelyhierarchizing the predetermined type of state amount on each node,wherein when a node relative floor reaction force Fn_rel(n=1, 2, . . . )hierarchically relativized on each node is determined on the basis of afloor reaction force Fn(n=1, 2, . . . ) acting on each of the groundcontact portions, which are the leaf nodes, and when a vector (Fa1_rel,Fa2_rel, . . . , Far_rel) having the node relative floor reaction forcesof all child nodes aj (j=1, 2, . . . , r, r denotes the total number ofthe child nodes of an n-th node) of an n-th node as its elements, whichis an arbitrary node having child nodes, is represented by a linearlinkage of a plurality of predetermined mutually independent vectorsR(j)(j=1, 2, . . . , r−1) that are all orthogonal to a vector (Wa1, Wa2,. . . , War) having the weights of all child nodes of the n-th node asits elements, a vector having a coefficient of the linear linkage as itselement is defined as a node expansion floor reaction force momentMn_exp of the n-th node, and wherein when a node relative heightZn_rel(n=1, 2, . . . ) hierarchically relativized on each node isdetermined on the basis of a height Zn(n=1, 2, . . . ) of the groundcontact surface of each of the ground contact portions, which are theleaf nodes, and when a vector (Za1_rel, Za2_rel, . . . , Zar_rel) havingthe node relative heights of all child nodes aj (j=1, 2, . . . , r, rdenotes the total number of the child nodes of an n-th node) of the n-thnode as its elements is expressed by a linear linkage of the pluralityof the predetermined mutually independent vectors R(j)(j=1, 2, . . . ,r−1), a vector having a coefficient of the linear linkage as its elementis defined as a node expansion inclination angle θn of the n-th node,then, the floor configuration estimating means estimates the floorconfiguration parameter by using the node expansion floor reaction forcemoment Mn_exp(n=C) or the node expansion inclination angle θn(n=C) of atleast the C-th node.
 18. The control device for a mobile body accordingto claim 17, wherein when a B-th node, which is each node in the treestructure, is the leaf node, then the height of the ground contactsurface of the ground contact portion, which is the leaf node, isdefined as the height of the ground contact surface of the B-th node,and when the B-th node has child nodes, then a weighted mean value,which uses the weight, of the heights of the ground contact surfaces ofall child nodes of the B-th node, is defined as the height of the groundcontact surface of the B-th node, then the floor configuration parameterincludes a parameter that uses the node expansion inclination angle toindicate the relative height of the ground contact surface of each childnode of the C-th node.
 19. The control device for a mobile bodyaccording to claim 17, comprising a floor reaction force detecting meansfor detecting or estimating an actual floor reaction force acting oneach of the ground contact portions, wherein the node operationcontrolling means comprises means for sequentially determining the nodeexpansion moment on the basis of at least an actual floor reaction forceof each of the ground contact portions, and a means for sequentiallydetermining the node expansion inclination angle by multiplying thedetermined node expansion moment by a predetermined matrix, andestimates a new floor configuration parameter on the basis of thedetermined node expansion inclination angle and the past value of anestimated value of the floor configuration parameter.
 20. The controldevice for a mobile body according to claim 1, wherein each node in thetree structure has a set weight, and when a B-th node, which is eachnode in the tree structure, is the leaf node, then the height of aground contact portion, which is the leaf node, is defined as the heightof the B-th node, and when the B-th node has child nodes, then aweighted mean value, which uses the weight, of the heights of all childnodes of the B-th node is defined as the height of the B-th node, andwhen the node operation controlling means determines the correcteddesired motion for only one arbitrary node among the C-th nodes byadding a first correction of the desired relative heights of theplurality of ground contact portions belonging to that one node to thedesired motion, then the desired height of that one node in thecorrected desired motion is maintained at a desired height in thedesired motion.
 21. The control device for a mobile body according toclaim 20, wherein, on each node having child nodes in the treestructure, the weights of all child nodes of the each node are set suchthat the total sum of the weights of all the child nodes becomes one.22. The control device for a mobile body according to claim 20,comprising a means for variably setting the weight of the each node,wherein when leaf nodes corresponding to ground contact portions,respectively, that are not in contact with the ground are defined asD-th nodes, then the means for setting the weights sets the weight ofeach D-node or the weight of at least one of ancestor nodes of the D-thnode to zero.
 23. The control device for a mobile body according toclaim 20, wherein the ground contact portions are connected to the basebody of the mobile body in such a manner that they are movable withrespect to the base body, and when determining the corrected desiredmotion, the node operation controlling means determines the correcteddesired motion by further adding to the desired motion a correction of adesired posture of the base body to restrain slippage of the groundcontact portions belonging to the C-th node on a floor surface.
 24. Thecontrol device for a mobile body according to claim 23, wherein the nodeoperation controlling means determines the corrected desired motion suchthat the horizontal position of the overall center-of-gravity of themobile body in the corrected desired motion or the horizontal positionof a predetermined representative point of the base body of the mobilebody is substantially the same as the horizontal position in the desiredmotion.
 25. The control device for a mobile body according to claim 23,wherein the mobile body comprises a plurality of link mechanismsextended from its base body, and at least one link mechanism out of theplurality of link mechanisms has a joint provided at an intermediateportion between at least its distal portion and the end portion adjacentto the base body, the intermediate portion being the ground contactportion.
 26. The control device for a mobile body according to claim 25,wherein the intermediate portion is provided with an elastic member thatresiliently deforms when it comes in contact with the ground.
 27. Thecontrol device for a mobile body according to claim 23, wherein themobile body comprises a plurality of link mechanisms which are extendedfrom its base body and each of which has one or more joints, and atleast one link mechanism among the plurality of link mechanisms and thebase body are provided with the ground contact portions.
 28. The controldevice for a mobile body according to claim 27, wherein at least oneground contact portion of the base body is provided with an elasticmember that resiliently deforms when it comes in contact with theground.
 29. A control device for a mobile body comprising: a desiredmotion determining means for determining a desired motion of the mobilebody, having three or more ground contact portions connected through theintermediary of a plurality of joints such that mutual relative heightscan be manipulated, and an actual posture inclination detecting meansfor detecting or estimating the actual posture inclination of a basebody of the mobile body, the operation of the mobile body beingcontrolled to make the motion of the mobile body follow a desiredmotion, a weight being set for each node in a tree structure when theground contact portions are categorized in the tree structure mannersuch that all of the ground contact portions become leaf nodes and anintermediate node exists between the leaf nodes and a root node havingall the leaf nodes as descendant nodes, a node operation controllingmeans that determines a corrected desired motion obtained by adding atleast a first correction of desired relative heights of the plurality ofground contact portions belonging to a C-th node to the desired motionsuch that a posture inclination error is approximated to zero on thebasis of at least the posture inclination error, which is the differencebetween the actual posture inclination and a desired posture inclinationof the base body in the desired motion, and operates the joints suchthat the corrected desired motion that has been determined is satisfied,wherein a B-th node, which is each node in the tree structure, is theleaf node, then the height of the ground contact portion, which is theleaf node, is defined as the height of the B-th node, and when the B-thnode has child nodes, then a weighted mean value, which uses the weight,of the heights of all child nodes of the B-th node, is defined as theheight of the B-th node, when the node operation controlling meansdetermines the desired motion for only the C-th node by adding a firstcorrection of the desired relative heights of the plurality of groundcontact portions belonging to the C-th node to the desired motion, thenthe desired height of the C-th node in the corrected desired motion ismaintained at a desired height in the desired motion.
 30. The controldevice for a mobile body according to claim 29, wherein the groundcontact portions are connected to the base body of the mobile body insuch a manner that they are movable with respect to the base body, andwhen determining the corrected desired motion, the node operationcontrolling means determines the corrected desired motion by furtheradding to the desired motion a correction of a desired posture of thebase body to restrain slippage of the ground contact portions belongingto the C-th node on a floor surface.
 31. The control device for a mobilebody according to claim 30, wherein the mobile body comprises aplurality of link mechanisms extended from its base body, and at leastone link mechanism out of the plurality of link mechanisms has a jointprovided at an intermediate portion between at least its distal portionand the end portion adjacent to the base body, the intermediate portionbeing a ground contact portion belonging to the C-th node.
 32. Thecontrol device for a mobile body according to claim 31, wherein theintermediate portion is provided with an elastic member that resilientlydeforms when it comes in contact with the ground.
 33. The control devicefor a mobile body according to claim 30, wherein the mobile bodycomprises a plurality of link mechanisms which are extended from itsbase body and each of which has one or more joints, at least one linkmechanism among the plurality of link mechanisms and the base body areprovided with the ground contact portions, and ground contact portionsbelonging to the C-th node include at least the ground contact portionsof the base body.
 34. The control device for a mobile body according toclaim 33, wherein at least one ground contact portion of the base bodyis provided with an elastic member that resiliently deforms when itcomes in contact with the ground.
 35. The control device for a mobilebody according to claim 30, wherein the node operation controlling meansdetermines the corrected desired motion such that the horizontalposition of the overall center-of-gravity of the mobile body in thecorrected desired motion or the horizontal position of a predeterminedrepresentative point of the base body of the mobile body issubstantially the same as the horizontal position in the desired motion.36. The control device for a mobile body according to claim 29, wherein,on each node having child nodes in the tree structure, the weights ofall child nodes of the each node are set such that the total sum of theweights of all the child nodes becomes one.
 37. The control device for amobile body according to claim 29, comprising a means for variablysetting the weight of the each node, wherein when leaf nodescorresponding to ground contact portions, respectively, that are not incontact with the ground are defined as D-th nodes, then the means forsetting the weights sets the weight of each D-node or the weight of atleast one of ancestor nodes of the D-th node to zero.
 38. The controldevice for a mobile body according to claim 29, the ground contactportions being connected to the base body through the intermediary ofconnecting mechanisms such that they can be moved with respect to thebase body of the mobile body, comprising: a means for determining atranslational force component of a desired floor reaction force to beapplied to ground contact portions belonging to the C-th node when thejoint is operated such that the corrected desired motion is satisfied,wherein the node operation controlling means comprises a means forestimating a deformation amount that occurs at the connecting mechanismsbetween the base body and the ground contact portions belonging to theC-th node and the ground contact portions when the translational forcecomponent of the desired floor reaction force is applied to the groundcontact portions belonging to the C-th node on the basis of thetranslational force components of the desired floor reaction forces ofthe ground contact portions belonging to at least the C-th node, anddetermines the corrected desired motion by further adding a secondcorrection of the desired heights of the plurality of ground contactportions belonging to the C-th node, which is for canceling theestimated deformation amount, to the desired motion.
 39. The controldevice for a mobile body according to claim 29, comprising: a floorconfiguration estimating means for estimating a parameter that specifiesthe relative heights of the ground contact surfaces of a plurality ofground contact portions belonging to at least the C-th node as the floorconfiguration parameter that represents a floor configuration, whereinthe node operation controlling means determines the corrected desiredmotion by further adding a third correction of the desired relativeheights of a plurality of the ground contact portions belonging to theC-th node on the basis of an estimated value of a floor configurationparameter when determining the corrected desired motion.
 40. The controldevice for a mobile body according to claim 39, comprising: a floorreaction force detecting means for detecting or estimating an actualfloor reaction force that acts on each of the ground contact portions,wherein the floor configuration estimating means performs estimationwhile sequentially updating the floor configuration parameter on thebasis of a past value of an estimated value of the floor configurationparameter, a difference between the corrected desired motion and thedesired motion, the corrected motion, at least either a detected valueor an estimated value of an actual joint displacement, which is adisplacement amount of each joint of the mobile body, the actual postureinclination, and an actual floor reaction force of each of the groundcontact portions belonging to at least the C-th node.
 41. The controldevice for a mobile body according to claim 39, comprising: a floorreaction force detecting means for detecting or estimating an actualfloor reaction force that acts on each of the ground contact portions;and a means for sequentially determining at least an actual node floorreaction force, which is an actual value of the node floor reactionforce of each child node of the C-th node, from the actual floorreaction force of each ground contact portion belonging to the C-thnode, wherein when A-th nodes, which are the nodes in the treestructure, are the leaf nodes, then the floor reaction forces acting onthe ground contact portions, which are the leaf nodes, are defined asthe node floor reaction forces of the A-th nodes, and when the A-thnodes have child nodes, then the resultant force of the node floorreaction forces of all the child nodes of the A-th nodes is defined asthe node floor reaction force of the A-th nodes, and wherein the floorconfiguration estimating means performs estimation while sequentiallyupdating the floor configuration parameter on the basis of the pastvalue of an estimated value of the floor configuration parameter, thedifference between the corrected desired motion and the desired motion,the corrected motion, at least either a detected value or an estimatedvalue of an actual joint displacement, which is a displacement amount ofeach joint of the mobile body, the actual posture inclination, and arelative relationship among the actual node floor reaction forces of thechild nodes of at least the C-th node.
 42. The control device for amobile body according to claim 39, the ground contact portions beingconnected to the base body through the intermediary of connectingmechanisms such that they are movable with respect to the base body ofthe mobile body, comprising: a deformation amount detecting means fordetecting or estimating the amounts of deformations that occur at theconnecting mechanisms between the base body and the ground contactportions and at the ground contact portions, wherein the floorconfiguration estimating means performs estimation while sequentiallyupdating the floor configuration parameter on the basis of the pastvalue of an estimated value of the floor configuration parameter, adifference between the corrected desired motion and the desired motion,the corrected motion, at least either a detected value or an estimatedvalue of an actual joint displacement, which is a displacement amount ofeach joint of the mobile body, the actual posture inclination, and thedeformation amount associated with each ground contact portion belongingto at least the C-th node.
 43. The control device for a mobile bodyaccording to claim 42, comprising a floor reaction force detecting meansfor detecting or estimating an actual floor reaction force acting oneach of the ground contact portions belonging to the C-th node, whereinthe deformation amount detecting means estimates the deformation amounton the basis of the actual floor reaction force.
 44. The control devicefor a mobile body according to claim 39, wherein, each node in the treestructure has a set weight, and when a B-th node, which is each node inthe tree structure, is the leaf node, then the height of the groundcontact surface of the ground contact portion, which is the leaf node,is defined as the height of the ground contact surface of the B-th node,and when the B-th node has child nodes, then a weighted mean value,which uses the weight, of the heights of the ground contact surfaces ofall child nodes of the B-th node is defined as the height of the groundcontact surface of the B-th node, and when these definitions apply, thefloor configuration parameter estimated by the floor configurationestimating means is a parameter that specifies a relative relationshipof the heights of the ground contact surfaces of a plurality of childnodes of the C-th node.
 45. The control device for a mobile bodyaccording to claim 44, wherein when at least one of the child nodes ofthe C-th node is about to float, then the floor configuration estimatingmeans estimates the floor configuration parameter that specifies therelative relationship of the heights of the ground contact surfaces ofthe plurality of child nodes while retaining, at a fixed value, therelative height of the ground contact surface of the child node that isabout to float.
 46. The control device for a mobile body according toclaim 39, wherein the floor configuration estimating means estimates thefloor configuration parameter by using a low-pass filter to alleviatefluctuation in the floor configuration parameter.
 47. The control devicefor a mobile body according to claim 39, wherein a weight for estimatinga floor configuration is set on each node of the tree structure, whereinwhen a predetermined type of state amount is associated with each leafnode in the tree structure, the state amount of each node having childnodes is defined as a weighted mean value, which uses the weight, of thestate amounts of all child nodes of the node, a value obtained bysubtracting a state amount of a parent node of each node from the stateamount of the node is determined as a node relative state amount of thenode on the node except a root node, and zero is determined as arelative state amount of the root node, is defined as the processing forrelatively hierarchizing the predetermined type of state amount on eachnode, wherein a node relative floor reaction force Fn_rel(n=1, 2, . . .) hierarchically relativized on each node is determined on the basis ofa floor reaction force Fn(n=1, 2, . . . ) acting on each of the groundcontact portions, which are the leaf nodes, and when a vector (Fa1_rel,Fa2_rel, . . . , Far_rel) having the node relative floor reaction forcesof all child nodes aj (j=1, 2, . . . , r, r denotes the total number ofthe child nodes of an n-th node) of an n-th node as its elements, whichis an arbitrary node having child nodes, is represented by a linearlinkage of a plurality of predetermined mutually independent vectorsR(j)(j=1, 2, . . . , r−1) that are all orthogonal to a vector (Wa1, Wa2,. . . , War) having the weights of all child nodes of the n-th node asits elements, a vector having a coefficient of the linear linkage as itselement is defined as a node expansion floor reaction force momentMn_exp of the n-th node, and wherein a node relative height Zn_rel(n=1,2, . . . ) hierarchically relativized on each node is determined on thebasis of a height Zn(n=1, 2, . . . ) of the ground contact surface ofeach of the ground contact portions, which are the leaf nodes, and whena vector (Za1_rel, Za2_rel, . . . , Zar_rel) having the node relativeheights of all child nodes aj (j=1, 2, . . . r. r denotes the totalnumber of the child nodes of an n-th node) of the n-th node as itselements is expressed by a linear linkage of the plurality of thepredetermined mutually independent vectors R(j)(j=1, 2, . . . , r−1), avector having a coefficient of the linear linkage as its element isdefined as a node expansion inclination angle θn of the n-th node, then,the floor configuration estimating means estimates the floorconfiguration parameter by using the node expansion floor reaction forcemoment Mn_exp(n=C) or the node expansion inclination angle θn(n=C) of atleast the C-th node.
 48. The control device for a mobile body accordingto claim 47, wherein a B-th node, which is each node in the treestructure, is the leaf node, then the height of the ground contactsurface of the ground contact portion, which is the leaf node, isdefined as the height of the ground contact surface of the B-th node,and when the B-th node has child nodes, then a weighted mean value,which uses the weight, of the heights of the ground contact surfaces ofall child nodes of the B-th node, is defined as the height of the groundcontact surface of the B-th node, then the floor configuration parameterincludes a parameter that uses the node expansion inclination angle toindicate the relative height of the ground contact surface of each childnode of the C-th node.
 49. The control device for a mobile bodyaccording to claim 47, comprising a floor reaction force detecting meansfor detecting or estimating an actual floor reaction force acting oneach of the ground contact portions, wherein the node operationcontrolling means comprises means for sequentially determining the nodeexpansion moment on the basis of at least an actual floor reaction forceof each of the ground contact portions, and a means for sequentiallydetermining the node expansion inclination angle by multiplying thedetermined node expansion moment by a predetermined matrix, andestimates a new floor configuration parameter on the basis of thedetermined node expansion inclination angle and the past value of anestimated value of the floor configuration parameter.